Skip to content

Instantly share code, notes, and snippets.

@eqbal eqbal/
Created Sep 16, 2017

What would you like to do?
#!/usr/bin/env python
import math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from simple_arm.srv import *
def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):
tolerance = .05
result = abs(pos_j1 - goal_j1) <= abs(tolerance)
result = result and abs(pos_j2 - goal_j2) <= abs(tolerance)
return result
def clamp_at_boundaries(requested_j1, requested_j2):
clamped_j1 = requested_j1
clamped_j2 = requested_j2
min_j1 = rospy.get_param('~min_joint_1_angle', 0)
max_j1 = rospy.get_param('~max_joint_1_angle', 2*math.pi)
min_j2 = rospy.get_param('~min_joint_2_angle', 0)
max_j2 = rospy.get_param('~max_joint_2_angle', 2*math.pi)
if not min_j1 <= requested_j1 <= max_j1:
clamped_j1 = min(max(requested_j1, min_j1), max_j1)
rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',
min_j1, max_j1, clamped_j1)
if not min_j2 <= requested_j2 <= max_j2:
clamped_j2 = min(max(requested_j2, min_j2), max_j2)
rospy.logwarn('j2 is out of bounds, valid range (%s,%s), clamping to: %s',
min_j2, max_j2, clamped_j2)
return clamped_j1, clamped_j2
def move_arm(pos_j1, pos_j2):
time_elapsed =
while True:
joint_state = rospy.wait_for_message('/simple_arm/joint_states', JointState)
if at_goal(joint_state.position[0], pos_j1, joint_state.position[1], pos_j2):
time_elapsed = joint_state.header.stamp - time_elapsed
return time_elapsed
def handle_safe_move_request(req):
rospy.loginfo('GoToPositionRequest Received - j1:%s, j2:%s',
req.joint_1, req.joint_2)
clamp_j1, clamp_j2 = clamp_at_boundaries(req.joint_1, req.joint_2)
time_elapsed = move_arm(clamp_j1, clamp_j2)
return GoToPositionResponse(time_elapsed)
def mover_service():
service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)
if __name__ == '__main__':
j1_publisher = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
Float64, queue_size=10)
j2_publisher = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
Float64, queue_size=10)
except rospy.ROSInterruptException:
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.