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)
