def __init__(self):
self.received_state = False
if (not rospy.has_param("joints")):
rospy.logerr("No arm joints given.")
exit(0)
else:
self.joint_names = sorted(rospy.get_param("joints"))
rospy.loginfo("arm joints: %s", self.joint_names)
# read joint limits
self.joint_limits = []
for joint in self.joint_names:
if ((not rospy.has_param("limits/" + joint + "/min"))
or (not rospy.has_param("limits/" + joint + "/min"))):
rospy.logerr("No arm joint limits given.")
exit(0)
else:
limit = arm_navigation_msgs.msg.JointLimits()
limit.joint_name = joint
limit.min_position = rospy.get_param("limits/" + joint +
"/min")
limit.max_position = rospy.get_param("limits/" + joint +
"/max")
self.joint_limits.append(limit)
self.current_joint_configuration = [
0 for i in range(len(self.joint_names))
]
self.unit = "rad"
# subscriptions
rospy.Subscriber("joint_states", sensor_msgs.msg.JointState,
self.joint_states_callback)
# publications
self.pub_joint_positions = rospy.Publisher(
"position_command", brics_actuator.msg.JointPositions)
# action server
self.as_move_joint_direct = actionlib.SimpleActionServer(
"MoveToJointConfigurationDirect",
raw_arm_navigation.msg.MoveToJointConfigurationAction,
execute_cb=self.execute_cb_move_joint_config_direct)
self.as_move_cart_direct = actionlib.SimpleActionServer(
"MoveToCartesianPoseDirect",
raw_arm_navigation.msg.MoveToCartesianPoseAction,
execute_cb=self.execute_cb_move_cartesian_direct)
self.as_move_cart_rpy_sampled_direct = actionlib.SimpleActionServer(
"MoveToCartesianRPYSampledDirect",
raw_arm_navigation.msg.MoveToCartesianPoseAction,
execute_cb=self.execute_cb_move_cartesian_rpy_sampled_direct)
# additional classes
self.iks = SimpleIkSolver()
