incrementer_server.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import actionlib
4 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
5 from control_msgs.msg import JointTrajectoryControllerState as JTCS
6 from teleop_tools_msgs.msg import IncrementAction as TTIA
7 
8 
10  def __init__(self, controller_ns):
11  self._as = actionlib.SimpleActionServer("increment",
12  TTIA,
13  execute_cb=self._as_cb,
14  auto_start=False)
15  self._command_pub = rospy.Publisher("command",
16  JointTrajectory,
17  queue_size=1)
18  state = rospy.wait_for_message("state", JTCS)
19  self._value = state.actual.positions
20  self._goal = JointTrajectory()
21  self._goal.joint_names = state.joint_names
22  rospy.loginfo('Connected to ' + controller_ns)
23  self._as.start()
24 
25  def _as_cb(self, goal):
26  self.increment_by(goal.increment_by)
27  self._as.set_succeeded([])
28 
29  def increment_by(self, increment):
30  state = rospy.wait_for_message("state", JTCS)
31  self._value = state.actual.positions
32  self._value = [x + y for x, y in zip(self._value, increment)]
33  rospy.loginfo('Sent goal of ' + str(self._value))
34  point = JointTrajectoryPoint()
35  point.positions = self._value
36  point.time_from_start = rospy.Duration(0.1)
37  self._goal.points = [point]
38  self._command_pub.publish(self._goal)
39 
40 if __name__ == "__main__":
41  rospy.init_node('incrementer_server')
42  controller_namespace = 'spine_controller'
43  server = IncrementerServer(controller_namespace)
44  rospy.spin()


joy_teleop
Author(s): Paul Mathieu
autogenerated on Sat Jun 8 2019 17:58:59