You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

295 lines
12 KiB

2 years ago
  1. ## Example 6
  2. In this example, we will review a Python script that prints out and stores the effort values from a specified joint. If you are looking for a continuous print of the joint state efforts while Stretch is in action, then you can use the [rostopic command-line tool](http://wiki.ros.org/rostopic) shown in the [Internal State of Stretch Tutorial](internal_state_of_stretch.md).
  3. <p align="center">
  4. <img src="https://raw.githubusercontent.com/hello-robot/stretch_tutorials/main/images/effort_sensing.gif"/>
  5. </p>
  6. Begin by running the following command in the terminal in a terminal.
  7. ```bash
  8. # Terminal 1
  9. roslaunch stretch_core stretch_driver.launch
  10. ```
  11. Switch the mode to *position* mode using a rosservice call. Then run the [effort_sensing.py](https://github.com/hello-robot/stretch_tutorials/blob/main/src/effort_sensing.py) node.
  12. ```bash
  13. # Terminal 2
  14. rosservice call /switch_to_position_mode
  15. cd catkin_ws/src/stretch_tutorials/src/
  16. python effort_sensing.py
  17. ```
  18. This will send a `FollowJointTrajectory` command to move Stretch's arm or head while also printing the effort of the lift.
  19. ### The Code
  20. ```python
  21. #!/usr/bin/env python
  22. import rospy
  23. import time
  24. import actionlib
  25. import os
  26. import csv
  27. from datetime import datetime
  28. from control_msgs.msg import FollowJointTrajectoryGoal
  29. from trajectory_msgs.msg import JointTrajectoryPoint
  30. from sensor_msgs.msg import JointState
  31. import hello_helpers.hello_misc as hm
  32. class JointActuatorEffortSensor(hm.HelloNode):
  33. """
  34. A class that sends multiple joint trajectory goals to a single joint.
  35. """
  36. def __init__(self, export_data=False):
  37. """
  38. Function that initializes the subscriber,and other features.
  39. :param self: The self reference.
  40. :param export_data: A boolean message type.
  41. """
  42. hm.HelloNode.__init__(self)
  43. self.sub = rospy.Subscriber('joint_states', JointState, self.callback)
  44. self.joints = ['joint_lift']
  45. self.joint_effort = []
  46. self.save_path = '/home/hello-robot/catkin_ws/src/stretch_tutorials/stored_data'
  47. self.export_data = export_data
  48. def callback(self, msg):
  49. """
  50. Callback function to update and store JointState messages.
  51. :param self: The self reference.
  52. :param msg: The JointState message.
  53. """
  54. self.joint_states = msg
  55. def issue_command(self):
  56. """
  57. Function that makes an action call and sends joint trajectory goals
  58. to a single joint.
  59. :param self: The self reference.
  60. """
  61. trajectory_goal = FollowJointTrajectoryGoal()
  62. trajectory_goal.trajectory.joint_names = self.joints
  63. point0 = JointTrajectoryPoint()
  64. point0.positions = [0.9]
  65. trajectory_goal.trajectory.points = [point0]
  66. trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
  67. trajectory_goal.trajectory.header.frame_id = 'base_link'
  68. self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback)
  69. rospy.loginfo('Sent position goal = {0}'.format(trajectory_goal))
  70. self.trajectory_client.wait_for_result()
  71. def feedback_callback(self,feedback):
  72. """
  73. The feedback_callback function deals with the incoming feedback messages
  74. from the trajectory_client. Although, in this function, we do not use the
  75. feedback information.
  76. :param self: The self reference.
  77. :param feedback: FollowJointTrajectoryActionFeedback message.
  78. """
  79. if 'wrist_extension' in self.joints:
  80. self.joints.remove('wrist_extension')
  81. self.joints.append('joint_arm_l0')
  82. current_effort = []
  83. for joint in self.joints:
  84. index = self.joint_states.name.index(joint)
  85. current_effort.append(self.joint_states.effort[index])
  86. if not self.export_data:
  87. print("name: " + str(self.joints))
  88. print("effort: " + str(current_effort))
  89. else:
  90. self.joint_effort.append(current_effort)
  91. def done_callback(self, status, result):
  92. """
  93. The done_callback function will be called when the joint action is complete.
  94. Within this function we export the data to a .txt file in the /stored_data directory.
  95. :param self: The self reference.
  96. :param status: status attribute from FollowJointTrajectoryActionResult message.
  97. :param result: result attribute from FollowJointTrajectoryActionResult message.
  98. """
  99. if status == actionlib.GoalStatus.SUCCEEDED:
  100. rospy.loginfo('Succeeded')
  101. else:
  102. rospy.loginfo('Failed')
  103. if self.export_data:
  104. file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p")
  105. completeName = os.path.join(self.save_path, file_name)
  106. with open(completeName, "w") as f:
  107. writer = csv.writer(f)
  108. writer.writerow(self.joints)
  109. writer.writerows(self.joint_effort)
  110. def main(self):
  111. """
  112. Function that initiates the issue_command function.
  113. :param self: The self reference.
  114. """
  115. hm.HelloNode.main(self, 'issue_command', 'issue_command', wait_for_first_pointcloud=False)
  116. rospy.loginfo('issuing command...')
  117. self.issue_command()
  118. time.sleep(2)
  119. if __name__ == '__main__':
  120. try:
  121. node = JointActuatorEffortSensor(export_data=True)
  122. node.main()
  123. except KeyboardInterrupt:
  124. rospy.loginfo('interrupt received, so shutting down')
  125. ```
  126. ### The Code Explained
  127. This code is similar to that of the [multipoint_command](https://github.com/hello-robot/stretch_tutorials/blob/main/src/multipoint_command.py) and [joint_state_printer](https://github.com/hello-robot/stretch_tutorials/blob/main/src/joint_state_printer.py) node. Therefore, this example will highlight sections that are different from those tutorials. Now let's break the code down.
  128. ```python
  129. #!/usr/bin/env python
  130. ```
  131. Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script.
  132. ```python
  133. import rospy
  134. import time
  135. import actionlib
  136. import os
  137. import csv
  138. from datetime import datetime
  139. from control_msgs.msg import FollowJointTrajectoryGoal
  140. from trajectory_msgs.msg import JointTrajectoryPoint
  141. from sensor_msgs.msg import JointState
  142. import hello_helpers.hello_misc as hm
  143. ```
  144. You need to import rospy if you are writing a ROS [Node](http://wiki.ros.org/Nodes). Import the `FollowJointTrajectoryGoal` from the `control_msgs.msg` package to control the Stretch robot. Import `JointTrajectoryPoint` from the `trajectory_msgs` package to define robot trajectories. The `hello_helpers` package consists of a module that provides various Python scripts used across [stretch_ros](https://github.com/hello-robot/stretch_ros). In this instance, we are importing the `hello_misc` script.
  145. ```Python
  146. class JointActuatorEffortSensor(hm.HelloNode):
  147. """
  148. A class that sends multiple joint trajectory goals to a single joint.
  149. """
  150. def __init__(self, export_data=False):
  151. """
  152. Function that initializes the subscriber,and other features.
  153. :param self: The self reference.
  154. :param export_data: A boolean message type.
  155. """
  156. hm.HelloNode.__init__(self)
  157. ```
  158. The `JointActuatorEffortSensor ` class inherits the `HelloNode` class from `hm` and is initialized.
  159. ```python
  160. self.sub = rospy.Subscriber('joint_states', JointState, self.callback)
  161. self.joints = ['joint_lift']
  162. ```
  163. Set up a subscriber. We're going to subscribe to the topic "*joint_states*", looking for `JointState` messages. When a message comes in, ROS is going to pass it to the function "callback" automatically. Create a list of the desired joints you want to print.
  164. ```Python
  165. self.joint_effort = []
  166. self.save_path = '/home/hello-robot/catkin_ws/src/stretch_tutorials/stored_data'
  167. self.export_data = export_data
  168. ```
  169. Create an empty list to store the joint effort values. The *self.save_path* is the directory path where the .txt file of the effort values will be stored. You can change this path to a preferred directory. The *self.export_data* is a boolean and its default value is set to False. If set to True, then the joint values will be stored in a .txt file, otherwise, the values will be printed in the terminal where you ran the effort sensing node.
  170. ```python
  171. self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback)
  172. ```
  173. Include the feedback and done call back functions in the send goal function.
  174. ```python
  175. def feedback_callback(self,feedback):
  176. """
  177. The feedback_callback function deals with the incoming feedback messages
  178. from the trajectory_client. Although, in this function, we do not use the
  179. feedback information.
  180. :param self: The self reference.
  181. :param feedback: FollowJointTrajectoryActionFeedback message.
  182. """
  183. ```
  184. The feedback callback function takes in the `FollowJointTrajectoryActionFeedback` message as its argument.
  185. ```python
  186. if 'wrist_extension' in self.joints:
  187. self.joints.remove('wrist_extension')
  188. self.joints.append('joint_arm_l0')
  189. ```
  190. Use a conditional statement to replace `wrist_extenstion` to `joint_arm_l0`. This is because `joint_arm_l0` has the effort values that the `wrist_extension` is experiencing.
  191. ```python
  192. current_effort = []
  193. for joint in self.joints:
  194. index = self.joint_states.name.index(joint)
  195. current_effort.append(self.joint_states.effort[index])
  196. ```
  197. Create an empty list to store the current effort values. Then use a for loop to parse the joint names and effort values.
  198. ```python
  199. if not self.export_data:
  200. print("name: " + str(self.joints))
  201. print("effort: " + str(current_effort))
  202. else:
  203. self.joint_effort.append(current_effort)
  204. ```
  205. Use a conditional statement to print effort values in the terminal or store values into a list that will be used for exporting the data in a .txt file.
  206. ```Python
  207. def done_callback(self, status, result):
  208. """
  209. The done_callback function will be called when the joint action is complete.
  210. Within this function we export the data to a .txt file in the /stored_data directory.
  211. :param self: The self reference.
  212. :param status: status attribute from FollowJointTrajectoryActionResult message.
  213. :param result: result attribute from FollowJointTrajectoryActionResult message.
  214. """
  215. ```
  216. The done callback function takes in the `FollowJointTrajectoryActionResult` messages as its arguments.
  217. ```python
  218. if status == actionlib.GoalStatus.SUCCEEDED:
  219. rospy.loginfo('Succeeded')
  220. else:
  221. rospy.loginfo('Failed')
  222. ```
  223. Conditional statement to print whether the goal status in the `FollowJointTrajectoryActionResult` succeeded or failed.
  224. ```python
  225. if self.export_data:
  226. file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p")
  227. completeName = os.path.join(self.save_path, file_name)
  228. with open(completeName, "w") as f:
  229. writer = csv.writer(f)
  230. writer.writerow(self.joints)
  231. writer.writerows(self.joint_effort)
  232. ```
  233. A conditional statement is used to export the data to a .txt file. The file's name is set to the date and time the node was executed. That way, no previous files are overwritten.
  234. ### Plotting/Animating Effort Data
  235. <p align="center">
  236. <img src="https://raw.githubusercontent.com/hello-robot/stretch_tutorials/noetic/stored_data/2022-06-30_11:26:20-AM.png"/>
  237. </p>
  238. We added a simple python script, [stored_data_plotter.py](https://github.com/hello-robot/stretch_tutorials/blob/main/src/stored_data_plotter.py), to this package for plotting the stored data. **Note** you have to change the name of the file you wish to see in the python script. This is shown below:
  239. ```Python
  240. ####################### Copy the file name here! #######################
  241. file_name = '2022-06-30_11:26:20-AM'
  242. ```
  243. Once you have changed the file name, then run the following in a new command.
  244. ```bash
  245. cd catkin_ws/src/stretch_tutorials/src/
  246. python3 stored_data_plotter.py
  247. ```
  248. Because this is not a node, you don't need `roscore` to run this script. Please review the comments in the python script for additional guidance.