From d4684859599ed98d6989ad5a0b5da80d5faf8848 Mon Sep 17 00:00:00 2001 From: "Alan G. Sanchez" Date: Thu, 23 Jun 2022 10:53:36 -0700 Subject: [PATCH] Included comments in the python section of the tutorial. --- follow_joint_trajectory.md | 160 +++++++++++++++++++++---------------- 1 file changed, 90 insertions(+), 70 deletions(-) diff --git a/follow_joint_trajectory.md b/follow_joint_trajectory.md index d90ac45..18e0a1f 100644 --- a/follow_joint_trajectory.md +++ b/follow_joint_trajectory.md @@ -17,15 +17,15 @@ roslaunch stretch_core stretch_driver.launch In a new terminal type the following commands. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 stow_command.py +cd catkin_ws/src/stretch_ros_tutorials/src/ +python stow_command.py ``` This will send a FollowJointTrajectory command to stow Stretch's arm. ### The Code ```python -#!/usr/bin/env python3 +#!/usr/bin/env python import rospy from control_msgs.msg import FollowJointTrajectoryGoal @@ -34,38 +34,48 @@ import hello_helpers.hello_misc as hm import time class StowCommand(hm.HelloNode): + ''' + A class that sends a joint trajectory goal to stow the Stretch's arm. + ''' + def __init__(self): + hm.HelloNode.__init__(self) + + def issue_stow_command(self): + ''' + Function that makes an action call and sends stow postion goal. + :param self: The self reference. + ''' + stow_point = JointTrajectoryPoint() + stow_point.time_from_start = rospy.Duration(0.000) + stow_point.positions = [0.2, 0.0, 3.4] - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_stow_command(self): - stow_point = JointTrajectoryPoint() - stow_point.time_from_start = rospy.Duration(0.000) - stow_point.positions = [0.2, 0.0, 3.4] - - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - trajectory_goal.trajectory.points = [stow_point] - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' + trajectory_goal = FollowJointTrajectoryGoal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] + trajectory_goal.trajectory.points = [stow_point] + trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) + trajectory_goal.trajectory.header.frame_id = 'base_link' - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() + self.trajectory_client.send_goal(trajectory_goal) + rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) + self.trajectory_client.wait_for_result() - def main(self): - hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False) - rospy.loginfo('stowing...') - self.issue_stow_command() - time.sleep(2) + def main(self): + ''' + Function that initiates stow_command function. + :param self: The self reference. + ''' + hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False) + rospy.loginfo('stowing...') + self.issue_stow_command() + time.sleep(2) if __name__ == '__main__': - try: - node = StowCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + try: + node = StowCommand() + node.main() + except KeyboardInterrupt: + rospy.loginfo('interrupt received, so shutting down') ``` ### The Code Explained @@ -73,7 +83,7 @@ if __name__ == '__main__': Now let's break the code down. ```python -#!/usr/bin/env python3 +#!/usr/bin/env python ``` 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. @@ -156,15 +166,15 @@ roslaunch stretch_core stretch_driver.launch In a new terminal type the following commands. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 multipoint_command.py +cd catkin_ws/src/stretch_ros_tutorials/src/ +python multipoint_command.py ``` This will send a list of JointTrajectoryPoint's to move Stretch's arm. ### The Code ```python -#!/usr/bin/env python3 +#!/usr/bin/env python import rospy import time @@ -173,54 +183,64 @@ from trajectory_msgs.msg import JointTrajectoryPoint import hello_helpers.hello_misc as hm class MultiPointCommand(hm.HelloNode): + """ + A class that sends multiple joint trajectory goals to the stretch robot. + """ + def __init__(self): + hm.HelloNode.__init__(self) - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_multipoint_command(self): - point0 = JointTrajectoryPoint() - point0.positions = [0.2, 0.0, 3.4] - point0.velocities = [0.2, 0.2, 2.5] - point0.accelerations = [1.0, 1.0, 3.5] + def issue_multipoint_command(self): + """ + Function that makes an action call and sends multiple joint trajectory goals. + :param self: The self reference. + """ + point0 = JointTrajectoryPoint() + point0.positions = [0.2, 0.0, 3.4] + point0.velocities = [0.2, 0.2, 2.5] + point0.accelerations = [1.0, 1.0, 3.5] - point1 = JointTrajectoryPoint() - point1.positions = [0.3, 0.1, 2.0] + point1 = JointTrajectoryPoint() + point1.positions = [0.3, 0.1, 2.0] - point2 = JointTrajectoryPoint() - point2.positions = [0.5, 0.2, -1.0] + point2 = JointTrajectoryPoint() + point2.positions = [0.5, 0.2, -1.0] - point3 = JointTrajectoryPoint() - point3.positions = [0.6, 0.3, 0.0] + point3 = JointTrajectoryPoint() + point3.positions = [0.6, 0.3, 0.0] - point4 = JointTrajectoryPoint() - point4.positions = [0.8, 0.2, 1.0] + point4 = JointTrajectoryPoint() + point4.positions = [0.8, 0.2, 1.0] - point5 = JointTrajectoryPoint() - point5.positions = [0.5, 0.1, 0.0] + point5 = JointTrajectoryPoint() + point5.positions = [0.5, 0.1, 0.0] - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' - - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() + trajectory_goal = FollowJointTrajectoryGoal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] + trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] + trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) + trajectory_goal.trajectory.header.frame_id = 'base_link' - def main(self): - hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False) - rospy.loginfo('issuing multipoint command...') - self.issue_multipoint_command() - time.sleep(2) + self.trajectory_client.send_goal(trajectory_goal) + rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) + self.trajectory_client.wait_for_result() + + def main(self): + """ + Function that initiates the multipoint_command function. + :param self: The self reference. + """ + hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False) + rospy.loginfo('issuing multipoint command...') + self.issue_multipoint_command() + time.sleep(2) if __name__ == '__main__': - try: - node = MultiPointCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + try: + node = MultiPointCommand() + node.main() + except KeyboardInterrupt: + rospy.loginfo('interrupt received, so shutting down') ```