incrementer_server.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 import actionlib
00004 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00005 from control_msgs.msg import JointTrajectoryControllerState as JTCS
00006 from teleop_tools_msgs.msg import IncrementAction as TTIA
00007 
00008 
00009 class IncrementerServer:
00010     def __init__(self, controller_ns):
00011         self._as = actionlib.SimpleActionServer("increment",
00012                                                 TTIA,
00013                                                 execute_cb=self._as_cb,
00014                                                 auto_start=False)
00015         self._command_pub = rospy.Publisher("command",
00016                                             JointTrajectory,
00017                                             queue_size=1)
00018         state = rospy.wait_for_message("state", JTCS)
00019         self._value = state.actual.positions
00020         self._goal = JointTrajectory()
00021         self._goal.joint_names = state.joint_names
00022         rospy.loginfo('Connected to ' + controller_ns)
00023         self._as.start()
00024 
00025     def _as_cb(self, goal):
00026         self.increment_by(goal.increment_by)
00027         self._as.set_succeeded([])
00028 
00029     def increment_by(self, increment):
00030         state = rospy.wait_for_message("state", JTCS)
00031         self._value = state.actual.positions
00032         self._value = [x + y for x, y in zip(self._value, increment)]
00033         rospy.loginfo('Sent goal of ' + str(self._value))
00034         point = JointTrajectoryPoint()
00035         point.positions = self._value
00036         point.time_from_start = rospy.Duration(0.1)
00037         self._goal.points = [point]
00038         self._command_pub.publish(self._goal)
00039 
00040 if __name__ == "__main__":
00041     rospy.init_node('incrementer_server')
00042     controller_namespace = 'spine_controller'
00043     server = IncrementerServer(controller_namespace)
00044     rospy.spin()


joy_teleop
Author(s): Paul Mathieu
autogenerated on Sun Apr 23 2017 02:40:17