def default(self, ci='unused'): js = JointTrajectoryControllerState() js.header = self.get_ros_header() js.joint_names = self.data.keys() js.actual.positions = self.data.values() js.actual.velocities = [0.0] * len(self.data) js.actual.accelerations = [0.0] * len(self.data) self.publish(js)
def default(self, ci="unused"): js = JointTrajectoryControllerState() js.header = self.get_ros_header() js.joint_names = self.data.keys() js.actual.positions = self.data.values() js.actual.velocities = [0.0] * len(self.data) js.actual.accelerations = [0.0] * len(self.data) self.publish(js)
def default(self, ci='unused'): js = JointTrajectoryControllerState() js.header = self.get_ros_header() # timestamp is not a joint joints = dict(self.data) del joints['timestamp'] js.joint_names = joints.keys() js.actual.positions = joints.values() js.actual.velocities = [0.0] * len(joints) js.actual.accelerations = [0.0] * len(joints) self.publish(js)
def default(self, ci='unused'): js = JointTrajectoryControllerState() js.header = self.get_ros_header() # timestamp is not a joint joints = dict(self.data) del joints['timestamp'] js.joint_names = joints.keys() js.actual.positions = joints.values() js.actual.velocities = [0.0] * len(joints) js.actual.accelerations = [0.0] * len(joints) self.publish(js)
def __init__(self, action_server, has_state=True, has_command=True, use_pr2_trajectory=True, joint_names=[]): self.joint_names = joint_names if use_pr2_trajectory: from pr2_controllers_msgs.msg import JointTrajectoryAction as action_def from pr2_controllers_msgs.msg import JointTrajectoryGoal as goal_def from pr2_controllers_msgs.msg import JointTrajectoryFeedback as feedback_def from pr2_controllers_msgs.msg import JointTrajectoryResult as result_def action = '/joint_trajectory_action' else: from control_msgs.msg import FollowJointTrajectoryAction as action_def from control_msgs.msg import FollowJointTrajectoryGoal as goal_def from control_msgs.msg import FollowJointTrajectoryFeedback as feedback_def from control_msgs.msg import FollowJointTrajectoryResult as result_def action = '/follow_joint_trajectory' self.feedback_def = feedback_def self.result_def = result_def self.as_name = action_server rospy.loginfo("Action server listening at: " + self.as_name + action) self._as = SimpleActionServer(self.as_name + action, action_def, execute_cb=self.cb, auto_start=False) self._as.start() # Has /server_name/state topic if has_state: if use_pr2_trajectory: from pr2_controllers_msgs.msg import JointTrajectoryControllerState else: from control_msgs.msg import JointTrajectoryControllerState self.last_state = JointTrajectoryControllerState() rospy.loginfo("Publishing state at: " + self.as_name + '/state') self.state_pub = rospy.Publisher(self.as_name + '/state', JointTrajectoryControllerState, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.1), self.state_timer_cb) # Has /server_name/command topic if has_command: from trajectory_msgs.msg import JointTrajectory rospy.loginfo("Listening to commands at: " + self.as_name + '/command') self.cmd_sub = rospy.Subscriber(self.as_name + '/command', JointTrajectory, self.cmd_cb, queue_size=1)