From 56d341fa57b157bae541f0b5ccd6b2e8ac4c14b6 Mon Sep 17 00:00:00 2001 From: Chintan Desai Date: Tue, 15 Feb 2022 00:06:41 -0500 Subject: [PATCH 01/10] Updated README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 8626e37..1aeb7da 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Introduction -This repo provides instructions on the installation and use of code on the Stretch RE1 robot. The goal is to provide a user familiar with ROS with the tools to operate a Stretch robot. +This branch provides instructions on the installation and use of code on the Stretch RE1 robot. The goal is to provide a user familiar with ROS 2 with the tools to operate a Stretch robot. ## Stretch ROS Tutorials 1. [Getting Started](getting_started.md) @@ -20,7 +20,7 @@ This repo provides instructions on the installation and use of code on the Stret ## Other ROS Examples -To help get you get started on your software development, here are examples of nodes to have the stretch perform simple tasks. +To help you get started on your software development, here are examples of nodes to have the stretch perform simple tasks. 1. [Teleoperate Stretch with a Node](example_1.md) - Use a python script that sends velocity commands. 2. [Filter Laser Scans](example_2.md) - Publish new scan ranges that are directly in front of Stretch. From 10d54e3ca8f91a5626e477d068a654c38764d005 Mon Sep 17 00:00:00 2001 From: Chintan Desai Date: Thu, 17 Feb 2022 13:53:22 -0500 Subject: [PATCH 02/10] Updated for ROS2 - WIP --- src/move.py | 41 ++++++++++++++++++++++++++++------------- 1 file changed, 28 insertions(+), 13 deletions(-) diff --git a/src/move.py b/src/move.py index 02886ac..aa44779 100755 --- a/src/move.py +++ b/src/move.py @@ -1,16 +1,21 @@ #!/usr/bin/env python -# Every python controller needs these lines -import rospy +### This node is still WIP, proceed with caution -# The Twist message is used to send velocities to the robot. +# Import rclpy so its Node class can be used. +import threading +import rclpy +from rclpy.node import Node + +# Imports the built-in Twist message type that the node uses to structure the velocity data that it passes on the topic. from geometry_msgs.msg import Twist -class Move: +# The Move class is created, which inherits from (or is a subclass of) Node +class Move(Node): def __init__(self): # Setup a publisher that will send the velocity commands for the Stretch # This will publish on a topic called "/stretch/cmd_vel" with a message type Twist. - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo + self.publisher = self.create_publisher(Twist, '/stretch/cmd_vel', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo def move_forward(self): # Make a Twist message. We're going to set all of the elements, since we @@ -30,23 +35,30 @@ class Move: command.angular.y = 0.0 command.angular.z = 0.0 - # Publish the Twist commands - self.pub.publish(command) + # Publish the Twist message + self.publisher.publish(command) if __name__ == '__main__': + # First the rclpy library is initialized + rclpy.init(args=args) + # Initialize the node, and call it "move". - rospy.init_node('move') - + node = rclpy.create_node('move') + # Setup Move class to base_motion base_motion = Move() - - # Rate allows us to control the (approximate) rate at which we publish things. + + # Spin in a separate thread + thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True) + thread.start() + + # create_rate allows us to control the (approximate) rate at which we publish things. # For this example, we want to publish at 10Hz. - rate = rospy.Rate(10) + rate = node.create_rate(10) # This will loop until ROS shuts down the node. This can be done on the # command line with a ctrl-C, or automatically from roslaunch. - while not rospy.is_shutdown(): + while rclpy.ok(): # Run the move_foward function in the Move class base_motion.move_forward() @@ -55,3 +67,6 @@ if __name__ == '__main__': # available CPU resources. This will also add a lot of network traffic, # possibly slowing down other things. rate.sleep() + + rclpy.shutdown() + thread.join() From 6b7b04eb49836c7c6f71f599e78e58d2ef920b75 Mon Sep 17 00:00:00 2001 From: Chintan Desai Date: Thu, 17 Feb 2022 22:35:05 -0500 Subject: [PATCH 03/10] Default behavior is to rotate in place --- src/move.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/move.py b/src/move.py index aa44779..3bf8848 100755 --- a/src/move.py +++ b/src/move.py @@ -25,7 +25,7 @@ class Move(Node): # A Twist has three linear velocities (in meters per second), along each of the axes. # For Stretch, it will only pay attention to the x velocity, since it can't # directly move in the y direction or the z direction. - command.linear.x = 0.1 + command.linear.x = 0.0 command.linear.y = 0.0 command.linear.z = 0.0 @@ -33,7 +33,7 @@ class Move(Node): # The Stretch will only respond to rotations around the z (vertical) axis. command.angular.x = 0.0 command.angular.y = 0.0 - command.angular.z = 0.0 + command.angular.z = 1.0 # Publish the Twist message self.publisher.publish(command) From 681d8d047f3cdae8c691ea0d52d88fa17c3ce27a Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Fri, 18 Feb 2022 11:10:45 -0500 Subject: [PATCH 04/10] ported move.py to ROS2 --- src/move.py | 65 +++++++++++++++++++++++------------------------------ 1 file changed, 28 insertions(+), 37 deletions(-) diff --git a/src/move.py b/src/move.py index 3bf8848..de6dec5 100755 --- a/src/move.py +++ b/src/move.py @@ -1,9 +1,9 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 ### This node is still WIP, proceed with caution # Import rclpy so its Node class can be used. -import threading +from time import sleep import rclpy from rclpy.node import Node @@ -13,11 +13,9 @@ from geometry_msgs.msg import Twist # The Move class is created, which inherits from (or is a subclass of) Node class Move(Node): def __init__(self): - # Setup a publisher that will send the velocity commands for the Stretch - # This will publish on a topic called "/stretch/cmd_vel" with a message type Twist. - self.publisher = self.create_publisher(Twist, '/stretch/cmd_vel', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo + print("Starting to move in circle") - def move_forward(self): + def move_around(self): # Make a Twist message. We're going to set all of the elements, since we # can't depend on them defaulting to safe values. command = Twist() @@ -33,40 +31,33 @@ class Move(Node): # The Stretch will only respond to rotations around the z (vertical) axis. command.angular.x = 0.0 command.angular.y = 0.0 - command.angular.z = 1.0 + command.angular.z = 0.5 # Publish the Twist message - self.publisher.publish(command) + self.publisher_.publish(command) + + def main(self, args=None): + # First the rclpy library is initialized + rclpy.init(args=args) + + # Initialize the node, and call it "move". + node = rclpy.create_node('move') + # Setup a publisher that will send the velocity commands for the Stretch + # This will publish on a topic called "/stretch/cmd_vel" with a message type Twist. + self.publisher_ = node.create_publisher(Twist, '/stretch/cmd_vel', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo + -if __name__ == '__main__': - # First the rclpy library is initialized - rclpy.init(args=args) - - # Initialize the node, and call it "move". - node = rclpy.create_node('move') + # This will loop until ROS shuts down the node. This can be done on the + # command line with a ctrl-C, or automatically from roslaunch. + while rclpy.ok(): + # Run the move_foward function in the Move class + base_motion.move_around() + sleep(1.0) + base_motion.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': # Setup Move class to base_motion base_motion = Move() - - # Spin in a separate thread - thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True) - thread.start() - - # create_rate allows us to control the (approximate) rate at which we publish things. - # For this example, we want to publish at 10Hz. - rate = node.create_rate(10) - - # This will loop until ROS shuts down the node. This can be done on the - # command line with a ctrl-C, or automatically from roslaunch. - while rclpy.ok(): - # Run the move_foward function in the Move class - base_motion.move_forward() - - # Do an idle wait to control the publish rate. If we don't control the - # rate, the node will publish as fast as it can, and consume all of the - # available CPU resources. This will also add a lot of network traffic, - # possibly slowing down other things. - rate.sleep() - - rclpy.shutdown() - thread.join() + base_motion.main() From 1e47f6475879a3d80b7e87eddde365a918655ced Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Thu, 21 Apr 2022 18:52:25 -0400 Subject: [PATCH 05/10] Ported filtered laser scan to ROS 2 --- src/scan_filter.py | 50 +++++++++++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 20 deletions(-) diff --git a/src/scan_filter.py b/src/scan_filter.py index e3028a8..590a656 100755 --- a/src/scan_filter.py +++ b/src/scan_filter.py @@ -1,13 +1,15 @@ #!/usr/bin/env python # Every python controller needs these lines -import rospy +import rclpy +from rclpy.node import Node from numpy import linspace, inf from math import sin + # We're going to subscribe to a LaserScan message. from sensor_msgs.msg import LaserScan -class Scanfilter: +class Scanfilter(Node): def __init__(self): # We're going to assume that the robot is pointing up the x-axis, so that # any points with y coordinates further than half of the defined @@ -15,15 +17,6 @@ class Scanfilter: self.width = 1 self.extent = self.width / 2.0 - # Set up a subscriber. We're going to subscribe to the topic "scan", - # looking for LaserScan messages. When a message comes in, ROS is going - # to pass it to the function "callback" automatically. - self.sub = rospy.Subscriber('/scan', LaserScan, self.callback) - - # Set up a publisher. This will publish on a topic called "filtered_scan", - # with a LaserScan message type. - self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10) - def callback(self,msg): # Figure out the angles of the scan. We're going to do this each time, in case we're subscribing to more than one # laser, with different numbers of beams. @@ -39,14 +32,31 @@ class Scanfilter: msg.ranges = new_ranges self.pub.publish(msg) -if __name__ == '__main__': - # Initialize the node, and call it "scan_filter". - rospy.init_node('scan_filter') + def main(self, args=None): + # First the rclpy library is initialized + rclpy.init(args=args) + + # Initialize the node, and call it "move". + node = rclpy.create_node('scan_filter') - # Setup Scanfilter class - Scanfilter() + # Set up a publisher. This will publish on a topic called "filtered_scan", + # with a LaserScan message type. + self.pub = node.create_publisher(LaserScan, '/filtered_scan', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo + + # Set up a subscriber. We're going to subscribe to the topic "scan", + # looking for LaserScan messages. When a message comes in, ROS is going + # to pass it to the function "callback" automatically. + self.sub = node.create_subscription(LaserScan, '/scan', self.callback, 10) + + # Give control to ROS. This will allow the callback to be called whenever new + # messages come in. If we don't put this line in, then the node will not work, + # and ROS will not process any messages. + rclpy.spin(node) + + self.destroy_node() + rclpy.shutdown() - # Give control to ROS. This will allow the callback to be called whenever new - # messages come in. If we don't put this line in, then the node will not work, - # and ROS will not process any messages. - rospy.spin() +if __name__ == '__main__': + # Setup Scanfilter class + sf = Scanfilter() + sf.main() From c49640916bf2da1c088b68968bf62e37e5368c26 Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Tue, 14 Jun 2022 11:02:29 -0700 Subject: [PATCH 06/10] Add ported ROS2 tutorials WIP --- src/avoider.py | 48 ++++++++------ src/marker.py | 44 ++++++++----- src/multipoint_command.py | 135 +++++++++++++++++++------------------- src/stow_command.py | 19 +++--- 4 files changed, 135 insertions(+), 111 deletions(-) diff --git a/src/avoider.py b/src/avoider.py index 369b8fd..ae8c8f9 100755 --- a/src/avoider.py +++ b/src/avoider.py @@ -1,7 +1,8 @@ #!/usr/bin/env python # Every python controller needs these lines -import rospy +import rclpy +from rclpy.node import Node from numpy import linspace, inf, tanh from math import sin @@ -11,14 +12,8 @@ from geometry_msgs.msg import Twist # We're going to subscribe to a LaserScan message. from sensor_msgs.msg import LaserScan -class Avoider: +class Avoider(Node): def __init__(self): - # Set up a publisher and a subscriber. We're going to call the subscriber - # "scan", and filter the ranges similar to what we did in example 2. - # For the publisher, we're going to use the topic name /stretch/cmd_vel. - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo - self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed) - # We're going to assume that the robot is pointing up the x-axis, so that # any points with y coordinates further than half of the defined # width (1 meter) from the axis are not considered. @@ -29,7 +24,7 @@ class Avoider: # the closest obstacle measured in front of it. self.distance = 0.5 - # Alocate a Twist to use, and set everything to zero. We're going to do + # Allocate a Twist to use, and set everything to zero. We're going to do # this here, to save some time in the callback function, set_speed(). self.twist = Twist() self.twist.linear.x = 0.0 @@ -59,16 +54,31 @@ class Avoider: self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0 # Publish the command using the publisher - self.pub.publish(self.twist) + self.publisher_.publish(self.twist) -if __name__ == '__main__': - # Initialize the node, and call it "avoider". - rospy.init_node('avoider') + def main(): + # First the rclpy library is initialized + rclpy.init(args=args) + + # Initialize the node, and call it "avoider". + node = rclpy.create_node('avoider') + + # Set up a publisher and a subscriber. We're going to call the subscriber + # "scan", and filter the ranges similar to what we did in example 2. + # For the publisher, we're going to use the topic name /stretch/cmd_vel. + self.publisher_ = node.create_publisher(Twist, '/stretch/cmd_vel', 1) #/stretch_diff_drive_controller/cmd_vel for gazebo + self.subscriber_ = node.create_subscription(LaserScan, '/scan', self.set_speed, 10) - # Setup Avoider class - Avoider() + # Give control to ROS. This will allow the callback to be called whenever new + # messages come in. If we don't put this line in, then the node will not work, + # and ROS will not process any messages. + rclpy.spin(node) + + self.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + # Setup Avoider object + avoider = Avoider() + avoider.main() - # Give control to ROS. This will allow the callback to be called whenever new - # messages come in. If we don't put this line in, then the node will not work, - # and ROS will not process any messages. - rospy.spin() diff --git a/src/marker.py b/src/marker.py index 411d1b3..b6a69b4 100755 --- a/src/marker.py +++ b/src/marker.py @@ -4,23 +4,22 @@ # it drives around the world. # Import rospy and sys -import rospy +from time import sleep +import rclpy +from rclpy.node import Node # Import the Marker message type from the visualization_msgs package. from visualization_msgs.msg import Marker -class Balloon(): +class Balloon(Node): def __init__(self): - # Set up a publisher. We're going to publish on a topic called balloon. - self.publisher = rospy.Publisher('balloon', Marker, queue_size=10) - # Create a marker. Markers of all shapes share a common type. self.marker = Marker() # Set the frame ID and type. The frame ID is the frame in which the position of the marker # is specified. The type is the shape of the marker, detailed on the wiki page. self.marker.header.frame_id = '/base_link' - self.marker.header.stamp = rospy.Time() + self.marker.header.stamp = self.get_clock().now().to_msg() self.marker.type = self.marker.SPHERE # Each marker has a unique ID number. If you have more than one marker that @@ -56,19 +55,30 @@ class Balloon(): def publish_marker(self): # publisher the marker - self.publisher.publish(self.marker) + self.publisher_.publish(self.marker) + def main(): + # First the rclpy library is initialized + rclpy.init(args=args) + + # Initialize the node, as usual + node = rclpy.create_node('marker') -if __name__ == '__main__': - # Initialize the node, as usual - rospy.init_node('marker') + ballon = Balloon() - ballon = Balloon() + # Set up a publisher. We're going to publish on a topic called balloon. + self.publisher_ = rclpy.create_publisher(Marker, 'balloon', 10) + + # This will loop until ROS shuts down the node. This can be done on the + # command line with a ctrl-C, or automatically from roslaunch. + while rclpy.ok(): + ballon.publish_marker() + sleep(1.0) + + self.destroy_node() + rclpy.shutdown() - # Set a rate. - rate = rospy.Rate(10) - # Publish the marker at 10Hz. - while not rospy.is_shutdown(): - ballon.publish_marker() - rate.sleep() +if __name__ == '__main__': + balloon = Balloon() + balloon.main() diff --git a/src/multipoint_command.py b/src/multipoint_command.py index aff3ebd..5d4c9e2 100644 --- a/src/multipoint_command.py +++ b/src/multipoint_command.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 # Every python controller needs these lines -import rospy +import rclpy import time -# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to +# Import the FollowJointTrajectoryGoal from the control_msgs.action package to # control the Stretch robot. -from control_msgs.msg import FollowJointTrajectoryGoal +from control_msgs.action import FollowJointTrajectoryGoal # Import JointTrajectoryPoint from the trajectory_msgs package to define # robot trajectories. @@ -16,83 +16,84 @@ from trajectory_msgs.msg import JointTrajectoryPoint import hello_helpers.hello_misc as hm class MultiPointCommand(hm.HelloNode): + # Initialize the inhereted hm.Hellonode class. + def __init__(self): + hm.HelloNode.__init__(self) - # Initialize the inhereted hm.Hellonode class. - def __init__(self): - hm.HelloNode.__init__(self) + def issue_multipoint_command(self): + # Set point0 as a JointTrajectoryPoint(). + point0 = JointTrajectoryPoint() - def issue_multipoint_command(self): - # Set point0 as a JointTrajectoryPoint(). - point0 = JointTrajectoryPoint() + # Provide desired positions of lift, wrist extension, and yaw of + # the wrist (in meters). + point0.positions = [0.2, 0.0, 3.4] - # Provide desired positions of lift, wrist extension, and yaw of - # the wrist (in meters). - point0.positions = [0.2, 0.0, 3.4] + # Provide desired velocity of the lift (m/s), wrist extension (m/s), + # and wrist yaw (rad/s). + # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s! + point0.velocities = [0.2, 0.2, 2.5] - # Provide desired velocity of the lift (m/s), wrist extension (m/s), - # and wrist yaw (rad/s). - # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s! - point0.velocities = [0.2, 0.2, 2.5] + # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), + # and wrist yaw (rad/s^2). + point0.accelerations = [1.0, 1.0, 3.5] - # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), - # and wrist yaw (rad/s^2). - point0.accelerations = [1.0, 1.0, 3.5] + # Set positions for the following 5 trajectory points. + # IMPORTANT NOTE: If you do not provide any velocities or accelerations for the lift + # or wrist extension, then they go to their default values. However, the + # Velocity and Acceleration of the wrist yaw will stay the same from the + # previous value unless updated. + point1 = JointTrajectoryPoint() + point1.positions = [0.3, 0.1, 2.0] - # Set positions for the following 5 trajectory points. - # IMPORTANT NOTE: If you do not provide any velocities or accelerations for the lift - # or wrist extension, then they go to their default values. However, the - # Velocity and Acceleration of the wrist yaw will stay the same from the - # previous value unless updated. - 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] + # Set trajectory_goal as a FollowJointTrajectoryGoal and define + # the joint names as a list. + trajectory_goal = FollowJointTrajectoryGoal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - # Set trajectory_goal as a FollowJointTrajectoryGoal and define - # the joint names as a list. - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - - # Then trajectory_goal.trajectory.points is defined by a list of the joint - # trajectory points - trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] + # Then trajectory_goal.trajectory.points is defined by a list of the joint + # trajectory points + trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] # Specify the coordinate frame that we want (base_link) and set the time to be now. - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' - - # Make the action call and send the goal. The last line of code waits - # for the result before it exits the python script. - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - # Create a funcion, main(), to do all of the setup the hm.HelloNode class - # and issue the stow command. - def main(self): - # The arguments of the main function of the hm.HelloNode class are the - # node_name, node topic namespace, and boolean (default value is true). - 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) + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory_goal.trajectory.header.frame_id = 'base_link' + + # Make the action call and send the goal. The last line of code waits + # for the result before it exits the python script. + self.trajectory_client.send_goal_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) + self.trajectory_client.wait_for_result() + + # Create a funcion, main(), to do all of the setup the hm.HelloNode class + # and issue the stow command. + def main(self): + rclpy.init() + # The arguments of the main function of the hm.HelloNode class are the + # node_name, node topic namespace, and boolean (default value is true). + hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False) + node.get_logger().info('issuing multipoint command...') + self.issue_multipoint_command() + time.sleep(2) if __name__ == '__main__': - try: - # Initialize the MultiPointCommand() class and set it to node and run the - # main() function. - node = MultiPointCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + try: + # Initialize the MultiPointCommand() class and set it to node and run the + # main() function. + node = MultiPointCommand() + node.main() + except KeyboardInterrupt: + node.get_logger().info('Interrupt received, so shutting down') + diff --git a/src/stow_command.py b/src/stow_command.py index 480b47b..40345db 100755 --- a/src/stow_command.py +++ b/src/stow_command.py @@ -1,12 +1,13 @@ #!/usr/bin/env python3 # Every python controller needs these lines -import rospy +import rclpy import time +from rclpy.duration import Duration # Import the FollowJointTrajectoryGoal from the control_msgs.msg package to # control the Stretch robot. -from control_msgs.msg import FollowJointTrajectoryGoal +from control_msgs.action import FollowJointTrajectoryGoal # Import JointTrajectoryPoint from the trajectory_msgs package to define # robot trajectories. @@ -24,7 +25,8 @@ class StowCommand(hm.HelloNode): def issue_stow_command(self): # Set stow_point as a JointTrajectoryPoint(). stow_point = JointTrajectoryPoint() - stow_point.time_from_start = rospy.Duration(0.000) + duration = Duration(seconds=0.0) + stow_point.time_from_start = duration.to_msg() # Provide desired positions of lift, wrist extension, and yaw of # the wrist (in meters). @@ -40,22 +42,23 @@ class StowCommand(hm.HelloNode): trajectory_goal.trajectory.points = [stow_point] # Specify the coordinate frame that we want (base_link) and set the time to be now. - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() trajectory_goal.trajectory.header.frame_id = 'base_link' # Make the action call and send the goal. The last line of code waits # for the result before it exits the python script. - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) + self.trajectory_client.send_goal_async(trajectory_goal) + node.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) self.trajectory_client.wait_for_result() # Create a funcion, main(), to do all of the setup the hm.HelloNode class # and issue the stow command. def main(self): + rclpy.init() # The arguments of the main function of the hm.HelloNode class are the # node_name, node topic namespace, and boolean (default value is true). hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False) - rospy.loginfo('stowing...') + node.get_logger().info('stowing...') self.issue_stow_command() time.sleep(2) @@ -67,4 +70,4 @@ if __name__ == '__main__': node = StowCommand() node.main() except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + node.get_logger().info('interrupt received, so shutting down') From a6b0887c1e6ea34a2ded82a54387ba71e117c7ba Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Fri, 24 Jun 2022 13:16:21 -0400 Subject: [PATCH 07/10] Add TF and FK tutorials WIP --- src/forward_kinematics.py | 56 +++++++++++++++++++++++++++++ src/tf_broadcaster.py | 75 +++++++++++++++++++++++++++++++++++++++ src/tf_listener.py | 57 +++++++++++++++++++++++++++++ 3 files changed, 188 insertions(+) create mode 100644 src/forward_kinematics.py create mode 100644 src/tf_broadcaster.py create mode 100644 src/tf_listener.py diff --git a/src/forward_kinematics.py b/src/forward_kinematics.py new file mode 100644 index 0000000..5f74e65 --- /dev/null +++ b/src/forward_kinematics.py @@ -0,0 +1,56 @@ +import rclpy +import math +import numpy as np +from rclpy.node import Node + +class ForwardKinematics(): + def __init__(self, lift_pos, arm_pos, wrist_pos): + # Get joint values as node parameters that can be set from CLI + self.a1 = 0 + self.alpha1 = 0 + self.d1 = 0.2 + self.theta1 = 0 + self.a2 = 0 + self.alpha2 = 0 + self.d2 = lift_pos + self.theta2 = 0 + self.a3 = 0 + self.alpha3 = -1.5708 + self.d3 = 0.5 + arm_pos + self.theta3 = -wrist_pos + self.a4 = 0 + self.alpha4 = 1.5708 + self.d4 = 0 + self.theta4 = 1.5708 + + def print_dh_table(self): + # Fancy print of the DH table here + return 0 + + def get_hm_matrix(self, a, alpha, d, theta): + # Angles are in radians + Ax = np.array([(math.cos(theta), -math.sin(theta)*math.cos(alpha), math.sin(theta)*math.sin(alpha), a*math.cos(theta)), + (math.sin(theta), math.cos(theta)*math.cos(alpha), -math.cos(theta)*math.sin(alpha), a*sin(theta)), + (0, math.sin(alpha), math.cos(alpha), d), + (0, 0, 0, 1)]) + return Ax + + def hm_matrices(self): + # Create numpy array to deal with matrix operations + A1 = get_hm_matrix(self.a1, self.alpha1, self.d1, self.theta1) + A2 = get_hm_matrix(self.a2, self.alpha2, self.d2, self.theta2) + A3 = get_hm_matrix(self.a3, self.alpha3, self.d3, self.theta3) + A4 = get_hm_matrix(self.a4, self.alpha4, self.d4, self.theta4) + + def compute_fk(self): + An = A1*A2*A3*A4 + return 0 + + def main(self): + self.print_dh_table() + + return 0 + +if __name__ == '__main__': + fk = ForwardKinematics(lift_pos=0, arm_pos=0, wrist_pos=0) + fk.main() diff --git a/src/tf_broadcaster.py b/src/tf_broadcaster.py new file mode 100644 index 0000000..6a32411 --- /dev/null +++ b/src/tf_broadcaster.py @@ -0,0 +1,75 @@ +from geometry_msgs.msg import TransformStamped + +import rclpy +from rclpy.node import Node +from tf2_ros import TransformBroadcaster +import tf_transformations + +# This node publishes three child static frames in reference to their parent frames as below: +# parent -> link_mast child -> fk_link_mast +# parent -> link_lift child -> fk_link_lift +# parent -> link_wrist_yaw child -> fk_link_wrist_yaw +# Tf frames are not defined according to the DH convention by default +# The new frames have been declared to make the computation of +# forward and inverse kinematics easier by defining the new frames +# according to the DH convention +class FixedFrameBroadcaster(Node): + + def __init__(self): + super().__init__('stretch_tf_broadcaster') + self.br = TransformBroadcaster(self) + self.timer = self.create_timer(0.1, self.broadcast_timer_callback) + + self.mast = TransformStamped() + self.mast.header.stamp = self.get_clock().now().to_msg() + self.mast.header.frame_id = 'link_mast' + self.mast.child_frame_id = 'fk_link_mast' + self.mast.transform.translation.x = 0.0 + self.mast.transform.translation.y = 0.0 + self.mast.transform.translation.z = 0.0 + q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) + self.mast.transform.rotation.x = q[0] + self.mast.transform.rotation.y = q[1] + self.mast.transform.rotation.z = q[2] + self.mast.transform.rotation.w = q[3] + + def broadcast_timer_callback(self): + self.br.sendTransform(self.mast) + + lift = TransformStamped() + lift.header.stamp = self.get_clock().now().to_msg() + lift.header.frame_id = 'link_lift' + lift.child_frame_id = 'fk_link_lift' + lift.transform.translation.x = 0.0 + lift.transform.translation.y = 2.0 + lift.transform.translation.z = 0.0 + q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) + lift.transform.rotation.x = q[0] + lift.transform.rotation.y = q[1] + lift.transform.rotation.z = q[2] + lift.transform.rotation.w = q[3] + self.br.sendTransform(lift) + + wrist = TransformStamped() + wrist.header.stamp = self.get_clock().now().to_msg() + wrist.header.frame_id = 'link_wrist_yaw' + wrist.child_frame_id = 'fk_link_wrist_yaw' + wrist.transform.translation.x = 0.0 + wrist.transform.translation.y = 2.0 + wrist.transform.translation.z = 0.0 + q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) + wrist.transform.rotation.x = q[0] + wrist.transform.rotation.y = q[1] + wrist.transform.rotation.z = q[2] + wrist.transform.rotation.w = q[3] + self.br.sendTransform(wrist) + +def main(): + rclpy.init() + node = FixedFrameBroadcaster() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + rclpy.shutdown() \ No newline at end of file diff --git a/src/tf_listener.py b/src/tf_listener.py new file mode 100644 index 0000000..511e2f0 --- /dev/null +++ b/src/tf_listener.py @@ -0,0 +1,57 @@ +import rclpy +from rclpy.node import Node + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +# This node prints the transformation between the fk_link_mast frame and a given target frame +# The default target frame is the link_grasp_center frame +# To change the target frame just pass it as an argument to the node, for ex. "target_frame:='fk_link_lift'" +class FrameListener(Node): + + def __init__(self): + super().__init__('stretch_tf_listener') + + # Declare and acquire `target_frame` parameter + self.declare_parameter('target_frame', 'link_grasp_center') + self.target_frame = self.get_parameter( + 'target_frame').get_parameter_value().string_value + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # Call on_timer function every second + self.timer = self.create_timer(1.0, self.on_timer) + + def on_timer(self): + # Store frame names in variables that will be used to + # compute transformations + from_frame_rel = self.target_frame + to_frame_rel = 'fk_link_mast' + + # Look up for the transformation between target_frame and link_mast frames + try: + now = rclpy.time.Time() + trans = self.tf_buffer.lookup_transform( + to_frame_rel, + from_frame_rel, + now) + except TransformException as ex: + self.get_logger().info( + f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}') + return + + self.get_logger().info( + f'the pose of target frame {from_frame_rel} with reference to {to_frame_rel} is: {trans}') + + +def main(): + rclpy.init() + node = FrameListener() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + rclpy.shutdown() \ No newline at end of file From 84fb4667551f312ca452806216fe1182bd37b490 Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Wed, 29 Jun 2022 22:28:43 -0400 Subject: [PATCH 08/10] Rosify the package --- package.xml | 18 ++++++++++++++++++ resource/stretch_ros_tutorials | 0 setup.cfg | 4 ++++ setup.py | 25 +++++++++++++++++++++++++ stretch_ros_tutorials/__init__.py | 0 test/test_copyright.py | 23 +++++++++++++++++++++++ test/test_flake8.py | 25 +++++++++++++++++++++++++ test/test_pep257.py | 23 +++++++++++++++++++++++ 8 files changed, 118 insertions(+) create mode 100644 package.xml create mode 100644 resource/stretch_ros_tutorials create mode 100644 setup.cfg create mode 100644 setup.py create mode 100644 stretch_ros_tutorials/__init__.py create mode 100644 test/test_copyright.py create mode 100644 test/test_flake8.py create mode 100644 test/test_pep257.py diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..e96c98f --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + stretch_ros_tutorials + 0.0.0 + TODO: Package description + chintan + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/resource/stretch_ros_tutorials b/resource/stretch_ros_tutorials new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..1cfd023 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/stretch_ros_tutorials +[install] +install_scripts=$base/lib/stretch_ros_tutorials diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..fc45f37 --- /dev/null +++ b/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'stretch_ros_tutorials' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='chintan', + maintainer_email='cdesai@hello-robot.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/stretch_ros_tutorials/__init__.py b/stretch_ros_tutorials/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From a0abe2de2060d731395d856bd18346c6bc7fd64a Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Sat, 2 Jul 2022 18:38:46 -0400 Subject: [PATCH 09/10] Add tested and working ROS 2 tutorials --- package.xml | 12 +- setup.py | 14 +- src/marker.py | 84 ----------- src/move.py | 63 --------- src/multipoint_command.py | 99 ------------- src/scan_filter.py | 62 -------- src/stow_command.py | 73 ---------- src/tf_broadcaster.py | 75 ---------- {src => stretch_ros_tutorials}/avoider.py | 63 ++++----- .../forward_kinematics.py | 0 stretch_ros_tutorials/marker.py | 78 ++++++++++ stretch_ros_tutorials/move.py | 57 ++++++++ stretch_ros_tutorials/multipoint_command.py | 133 ++++++++++++++++++ stretch_ros_tutorials/scan_filter.py | 68 +++++++++ stretch_ros_tutorials/stow_command.py | 105 ++++++++++++++ stretch_ros_tutorials/tf_broadcaster.py | 88 ++++++++++++ {src => stretch_ros_tutorials}/tf_listener.py | 14 +- 17 files changed, 589 insertions(+), 499 deletions(-) delete mode 100755 src/marker.py delete mode 100755 src/move.py delete mode 100644 src/multipoint_command.py delete mode 100755 src/scan_filter.py delete mode 100755 src/stow_command.py delete mode 100644 src/tf_broadcaster.py rename {src => stretch_ros_tutorials}/avoider.py (61%) rename {src => stretch_ros_tutorials}/forward_kinematics.py (100%) create mode 100755 stretch_ros_tutorials/marker.py create mode 100755 stretch_ros_tutorials/move.py create mode 100644 stretch_ros_tutorials/multipoint_command.py create mode 100755 stretch_ros_tutorials/scan_filter.py create mode 100755 stretch_ros_tutorials/stow_command.py create mode 100644 stretch_ros_tutorials/tf_broadcaster.py rename {src => stretch_ros_tutorials}/tf_listener.py (82%) diff --git a/package.xml b/package.xml index e96c98f..7b4db0e 100644 --- a/package.xml +++ b/package.xml @@ -3,9 +3,15 @@ stretch_ros_tutorials 0.0.0 - TODO: Package description - chintan - TODO: License declaration + This package contains a set of ROS tutorials for Stretch beginners + Chintan Desai + Apache License 2.0 + + rclpy + std_msgs + geometry_msgs + control_msgs + trajectory_msgs ament_copyright ament_flake8 diff --git a/setup.py b/setup.py index fc45f37..b089a91 100644 --- a/setup.py +++ b/setup.py @@ -13,13 +13,21 @@ setup( ], install_requires=['setuptools'], zip_safe=True, - maintainer='chintan', + maintainer='Chintan Desai', maintainer_email='cdesai@hello-robot.com', - description='TODO: Package description', - license='TODO: License declaration', + description='This package contains a set of ROS tutorials for Stretch beginners', + license='Apache License 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'stow_command = stretch_ros_tutorials.stow_command:main', + 'multipoint_command = stretch_ros_tutorials.multipoint_command:main', + 'marker = stretch_ros_tutorials.marker:main', + 'tf_broadcaster = stretch_ros_tutorials.tf_broadcaster:main', + 'tf_listener = stretch_ros_tutorials.tf_listener:main', + 'scan_filter = stretch_ros_tutorials.scan_filter:main', + 'avoider = stretch_ros_tutorials.avoider:main', + 'move = stretch_ros_tutorials.move:main', ], }, ) diff --git a/src/marker.py b/src/marker.py deleted file mode 100755 index b6a69b4..0000000 --- a/src/marker.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env python - -# This node will publish a spherical marker above the current robot position, as -# it drives around the world. - -# Import rospy and sys -from time import sleep -import rclpy -from rclpy.node import Node - -# Import the Marker message type from the visualization_msgs package. -from visualization_msgs.msg import Marker - -class Balloon(Node): - def __init__(self): - # Create a marker. Markers of all shapes share a common type. - self.marker = Marker() - - # Set the frame ID and type. The frame ID is the frame in which the position of the marker - # is specified. The type is the shape of the marker, detailed on the wiki page. - self.marker.header.frame_id = '/base_link' - self.marker.header.stamp = self.get_clock().now().to_msg() - self.marker.type = self.marker.SPHERE - - # Each marker has a unique ID number. If you have more than one marker that - # you want displayed at a given time, then each needs to have a unique ID - # number.If you publish a new marker with the same ID number and an existing - # marker, it will replace the existing marker with that ID number. - self.marker.id = 0 - - # Set the action. We can add, delete, or modify markers. - self.marker.action = self.marker.ADD - - # These are the size parameters for the marker. These will vary by marker type - self.marker.scale.x = 0.5 - self.marker.scale.y = 0.5 - self.marker.scale.z = 0.5 - - # Color, as an RGB triple, from 0 to 1. - self.marker.color.r = 1.0 - self.marker.color.g = 0.0 - self.marker.color.b = 0.0 - - # Alpha value, from 0 (invisible) to 1 (opaque). If you don't set this and - # it defaults to zero, then your marker will be invisible. - self.marker.color.a = 1.0 - - # Specify the pose of the marker. Since spheres are rotationally invarient, - # we're only going to specify the positional elements. As usual, these are - # in the coordinate frame named in frame_id. In this case, the position - # will always be directly above the robot, and will move with it. - self.marker.pose.position.x = 0.0 - self.marker.pose.position.y = 0.0 - self.marker.pose.position.z = 2.0 - - def publish_marker(self): - # publisher the marker - self.publisher_.publish(self.marker) - - def main(): - # First the rclpy library is initialized - rclpy.init(args=args) - - # Initialize the node, as usual - node = rclpy.create_node('marker') - - ballon = Balloon() - - # Set up a publisher. We're going to publish on a topic called balloon. - self.publisher_ = rclpy.create_publisher(Marker, 'balloon', 10) - - # This will loop until ROS shuts down the node. This can be done on the - # command line with a ctrl-C, or automatically from roslaunch. - while rclpy.ok(): - ballon.publish_marker() - sleep(1.0) - - self.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - balloon = Balloon() - balloon.main() diff --git a/src/move.py b/src/move.py deleted file mode 100755 index de6dec5..0000000 --- a/src/move.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python3 - -### This node is still WIP, proceed with caution - -# Import rclpy so its Node class can be used. -from time import sleep -import rclpy -from rclpy.node import Node - -# Imports the built-in Twist message type that the node uses to structure the velocity data that it passes on the topic. -from geometry_msgs.msg import Twist - -# The Move class is created, which inherits from (or is a subclass of) Node -class Move(Node): - def __init__(self): - print("Starting to move in circle") - - def move_around(self): - # Make a Twist message. We're going to set all of the elements, since we - # can't depend on them defaulting to safe values. - command = Twist() - - # A Twist has three linear velocities (in meters per second), along each of the axes. - # For Stretch, it will only pay attention to the x velocity, since it can't - # directly move in the y direction or the z direction. - command.linear.x = 0.0 - command.linear.y = 0.0 - command.linear.z = 0.0 - - # A Twist also has three rotational velocities (in radians per second). - # The Stretch will only respond to rotations around the z (vertical) axis. - command.angular.x = 0.0 - command.angular.y = 0.0 - command.angular.z = 0.5 - - # Publish the Twist message - self.publisher_.publish(command) - - def main(self, args=None): - # First the rclpy library is initialized - rclpy.init(args=args) - - # Initialize the node, and call it "move". - node = rclpy.create_node('move') - # Setup a publisher that will send the velocity commands for the Stretch - # This will publish on a topic called "/stretch/cmd_vel" with a message type Twist. - self.publisher_ = node.create_publisher(Twist, '/stretch/cmd_vel', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo - - - # This will loop until ROS shuts down the node. This can be done on the - # command line with a ctrl-C, or automatically from roslaunch. - while rclpy.ok(): - # Run the move_foward function in the Move class - base_motion.move_around() - sleep(1.0) - - base_motion.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - # Setup Move class to base_motion - base_motion = Move() - base_motion.main() diff --git a/src/multipoint_command.py b/src/multipoint_command.py deleted file mode 100644 index 5d4c9e2..0000000 --- a/src/multipoint_command.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python3 - -# Every python controller needs these lines -import rclpy -import time - -# Import the FollowJointTrajectoryGoal from the control_msgs.action package to -# control the Stretch robot. -from control_msgs.action import FollowJointTrajectoryGoal - -# Import JointTrajectoryPoint from the trajectory_msgs package to define -# robot trajectories. -from trajectory_msgs.msg import JointTrajectoryPoint - -# Import hello_misc script for handling trajecotry goals with an action client. -import hello_helpers.hello_misc as hm - -class MultiPointCommand(hm.HelloNode): - # Initialize the inhereted hm.Hellonode class. - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_multipoint_command(self): - # Set point0 as a JointTrajectoryPoint(). - point0 = JointTrajectoryPoint() - - # Provide desired positions of lift, wrist extension, and yaw of - # the wrist (in meters). - point0.positions = [0.2, 0.0, 3.4] - - # Provide desired velocity of the lift (m/s), wrist extension (m/s), - # and wrist yaw (rad/s). - # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s! - point0.velocities = [0.2, 0.2, 2.5] - - # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), - # and wrist yaw (rad/s^2). - point0.accelerations = [1.0, 1.0, 3.5] - - # Set positions for the following 5 trajectory points. - # IMPORTANT NOTE: If you do not provide any velocities or accelerations for the lift - # or wrist extension, then they go to their default values. However, the - # Velocity and Acceleration of the wrist yaw will stay the same from the - # previous value unless updated. - point1 = JointTrajectoryPoint() - point1.positions = [0.3, 0.1, 2.0] - - point2 = JointTrajectoryPoint() - point2.positions = [0.5, 0.2, -1.0] - - point3 = JointTrajectoryPoint() - point3.positions = [0.6, 0.3, 0.0] - - point4 = JointTrajectoryPoint() - point4.positions = [0.8, 0.2, 1.0] - - point5 = JointTrajectoryPoint() - point5.positions = [0.5, 0.1, 0.0] - - # Set trajectory_goal as a FollowJointTrajectoryGoal and define - # the joint names as a list. - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - - # Then trajectory_goal.trajectory.points is defined by a list of the joint - # trajectory points - trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] - - # Specify the coordinate frame that we want (base_link) and set the time to be now. - trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() - trajectory_goal.trajectory.header.frame_id = 'base_link' - - # Make the action call and send the goal. The last line of code waits - # for the result before it exits the python script. - self.trajectory_client.send_goal_async(trajectory_goal) - self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - # Create a funcion, main(), to do all of the setup the hm.HelloNode class - # and issue the stow command. - def main(self): - rclpy.init() - # The arguments of the main function of the hm.HelloNode class are the - # node_name, node topic namespace, and boolean (default value is true). - hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False) - node.get_logger().info('issuing multipoint command...') - self.issue_multipoint_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - # Initialize the MultiPointCommand() class and set it to node and run the - # main() function. - node = MultiPointCommand() - node.main() - except KeyboardInterrupt: - node.get_logger().info('Interrupt received, so shutting down') - diff --git a/src/scan_filter.py b/src/scan_filter.py deleted file mode 100755 index 590a656..0000000 --- a/src/scan_filter.py +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rclpy -from rclpy.node import Node -from numpy import linspace, inf -from math import sin - -# We're going to subscribe to a LaserScan message. -from sensor_msgs.msg import LaserScan - -class Scanfilter(Node): - def __init__(self): - # We're going to assume that the robot is pointing up the x-axis, so that - # any points with y coordinates further than half of the defined - # width (1 meter) from the axis are not considered. - self.width = 1 - self.extent = self.width / 2.0 - - def callback(self,msg): - # Figure out the angles of the scan. We're going to do this each time, in case we're subscribing to more than one - # laser, with different numbers of beams. - angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) - - # Work out the y coordinates of the ranges. - points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] - - # If we're close to the x axis, keep the range, otherwise use inf, which means "no return". - new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] - - # Substitute in the new ranges in the original message, and republish it. - msg.ranges = new_ranges - self.pub.publish(msg) - - def main(self, args=None): - # First the rclpy library is initialized - rclpy.init(args=args) - - # Initialize the node, and call it "move". - node = rclpy.create_node('scan_filter') - - # Set up a publisher. This will publish on a topic called "filtered_scan", - # with a LaserScan message type. - self.pub = node.create_publisher(LaserScan, '/filtered_scan', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo - - # Set up a subscriber. We're going to subscribe to the topic "scan", - # looking for LaserScan messages. When a message comes in, ROS is going - # to pass it to the function "callback" automatically. - self.sub = node.create_subscription(LaserScan, '/scan', self.callback, 10) - - # Give control to ROS. This will allow the callback to be called whenever new - # messages come in. If we don't put this line in, then the node will not work, - # and ROS will not process any messages. - rclpy.spin(node) - - self.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - # Setup Scanfilter class - sf = Scanfilter() - sf.main() diff --git a/src/stow_command.py b/src/stow_command.py deleted file mode 100755 index 40345db..0000000 --- a/src/stow_command.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python3 - -# Every python controller needs these lines -import rclpy -import time -from rclpy.duration import Duration - -# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to -# control the Stretch robot. -from control_msgs.action import FollowJointTrajectoryGoal - -# Import JointTrajectoryPoint from the trajectory_msgs package to define -# robot trajectories. -from trajectory_msgs.msg import JointTrajectoryPoint - -# Import hello_misc script for handling trajecotry goals with an action client. -import hello_helpers.hello_misc as hm - -class StowCommand(hm.HelloNode): - - # Initialize the inhereted hm.Hellonode class. - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_stow_command(self): - # Set stow_point as a JointTrajectoryPoint(). - stow_point = JointTrajectoryPoint() - duration = Duration(seconds=0.0) - stow_point.time_from_start = duration.to_msg() - - # Provide desired positions of lift, wrist extension, and yaw of - # the wrist (in meters). - stow_point.positions = [0.2, 0.0, 3.4] - - # Set trajectory_goal as a FollowJointTrajectoryGoal and define - # the joint names as a list. - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - - # Then trajectory_goal.trajectory.points is defined by the positions - # set in stow_point - trajectory_goal.trajectory.points = [stow_point] - - # Specify the coordinate frame that we want (base_link) and set the time to be now. - trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() - trajectory_goal.trajectory.header.frame_id = 'base_link' - - # Make the action call and send the goal. The last line of code waits - # for the result before it exits the python script. - self.trajectory_client.send_goal_async(trajectory_goal) - node.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - # Create a funcion, main(), to do all of the setup the hm.HelloNode class - # and issue the stow command. - def main(self): - rclpy.init() - # The arguments of the main function of the hm.HelloNode class are the - # node_name, node topic namespace, and boolean (default value is true). - hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False) - node.get_logger().info('stowing...') - self.issue_stow_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - # Initialize the StowCommand() class and set it to node and run the - # main() function. - node = StowCommand() - node.main() - except KeyboardInterrupt: - node.get_logger().info('interrupt received, so shutting down') diff --git a/src/tf_broadcaster.py b/src/tf_broadcaster.py deleted file mode 100644 index 6a32411..0000000 --- a/src/tf_broadcaster.py +++ /dev/null @@ -1,75 +0,0 @@ -from geometry_msgs.msg import TransformStamped - -import rclpy -from rclpy.node import Node -from tf2_ros import TransformBroadcaster -import tf_transformations - -# This node publishes three child static frames in reference to their parent frames as below: -# parent -> link_mast child -> fk_link_mast -# parent -> link_lift child -> fk_link_lift -# parent -> link_wrist_yaw child -> fk_link_wrist_yaw -# Tf frames are not defined according to the DH convention by default -# The new frames have been declared to make the computation of -# forward and inverse kinematics easier by defining the new frames -# according to the DH convention -class FixedFrameBroadcaster(Node): - - def __init__(self): - super().__init__('stretch_tf_broadcaster') - self.br = TransformBroadcaster(self) - self.timer = self.create_timer(0.1, self.broadcast_timer_callback) - - self.mast = TransformStamped() - self.mast.header.stamp = self.get_clock().now().to_msg() - self.mast.header.frame_id = 'link_mast' - self.mast.child_frame_id = 'fk_link_mast' - self.mast.transform.translation.x = 0.0 - self.mast.transform.translation.y = 0.0 - self.mast.transform.translation.z = 0.0 - q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) - self.mast.transform.rotation.x = q[0] - self.mast.transform.rotation.y = q[1] - self.mast.transform.rotation.z = q[2] - self.mast.transform.rotation.w = q[3] - - def broadcast_timer_callback(self): - self.br.sendTransform(self.mast) - - lift = TransformStamped() - lift.header.stamp = self.get_clock().now().to_msg() - lift.header.frame_id = 'link_lift' - lift.child_frame_id = 'fk_link_lift' - lift.transform.translation.x = 0.0 - lift.transform.translation.y = 2.0 - lift.transform.translation.z = 0.0 - q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) - lift.transform.rotation.x = q[0] - lift.transform.rotation.y = q[1] - lift.transform.rotation.z = q[2] - lift.transform.rotation.w = q[3] - self.br.sendTransform(lift) - - wrist = TransformStamped() - wrist.header.stamp = self.get_clock().now().to_msg() - wrist.header.frame_id = 'link_wrist_yaw' - wrist.child_frame_id = 'fk_link_wrist_yaw' - wrist.transform.translation.x = 0.0 - wrist.transform.translation.y = 2.0 - wrist.transform.translation.z = 0.0 - q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) - wrist.transform.rotation.x = q[0] - wrist.transform.rotation.y = q[1] - wrist.transform.rotation.z = q[2] - wrist.transform.rotation.w = q[3] - self.br.sendTransform(wrist) - -def main(): - rclpy.init() - node = FixedFrameBroadcaster() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - - rclpy.shutdown() \ No newline at end of file diff --git a/src/avoider.py b/stretch_ros_tutorials/avoider.py similarity index 61% rename from src/avoider.py rename to stretch_ros_tutorials/avoider.py index ae8c8f9..f9a5103 100755 --- a/src/avoider.py +++ b/stretch_ros_tutorials/avoider.py @@ -1,31 +1,33 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -# Every python controller needs these lines import rclpy from rclpy.node import Node from numpy import linspace, inf, tanh from math import sin -# The Twist message is used to send velocities to the robot. +# The Twist message is used to send velocities to the robot from geometry_msgs.msg import Twist -# We're going to subscribe to a LaserScan message. +# We're going to subscribe to a LaserScan message from sensor_msgs.msg import LaserScan + class Avoider(Node): def __init__(self): + super().__init__('stretch_avoider') + # We're going to assume that the robot is pointing up the x-axis, so that # any points with y coordinates further than half of the defined - # width (1 meter) from the axis are not considered. + # width (1 meter) from the axis are not considered self.width = 1 self.extent = self.width / 2.0 # We want the robot to drive foward or backwards until it is 0.5 m from - # the closest obstacle measured in front of it. + # the closest obstacle measured in front of it self.distance = 0.5 - # Allocate a Twist to use, and set everything to zero. We're going to do - # this here, to save some time in the callback function, set_speed(). + # Allocate a Twist to use, and set everything to zero here, + # to save some time in the callback function set_speed() self.twist = Twist() self.twist.linear.x = 0.0 self.twist.linear.y = 0.0 @@ -34,19 +36,25 @@ class Avoider(Node): self.twist.angular.y = 0.0 self.twist.angular.z = 0.0 + # Set up a publisher and a subscriber. We're going to call the subscriber + # "scan", and filter the ranges similar to what we did in example 2 + # For the publisher, we're going to use the topic name /stretch/cmd_vel + self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1) #/stretch_diff_drive_controller/cmd_vel for gazebo + self.subscriber_ = self.create_subscription(LaserScan, '/scan', self.set_speed, 10) + # Called every time we get a LaserScan message from ROS. - def set_speed(self,msg): + def set_speed(self, msg): # Figure out the angles of the scan. We're going to do this each time, in case we're subscribing to more than one - # laser, with different numbers of beams. + # laser, with different numbers of beams angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) - # Work out the y coordinates of the ranges. + # Work out the y coordinates of the ranges points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] - # If we're close to the x axis, keep the range, otherwise use inf, which means "no return". + # If we're close to the x axis, keep the range, otherwise use inf, which means "no return" new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] - # Calculate the difference of the closest measured scan and where we want the robot to stop. + # Calculate the difference of the closest measured scan and where we want the robot to stop error = min(new_ranges) - self.distance # Using hyperbolic tanget for speed regulation, with a threshold to stop @@ -56,29 +64,18 @@ class Avoider(Node): # Publish the command using the publisher self.publisher_.publish(self.twist) - def main(): - # First the rclpy library is initialized - rclpy.init(args=args) - - # Initialize the node, and call it "avoider". - node = rclpy.create_node('avoider') - # Set up a publisher and a subscriber. We're going to call the subscriber - # "scan", and filter the ranges similar to what we did in example 2. - # For the publisher, we're going to use the topic name /stretch/cmd_vel. - self.publisher_ = node.create_publisher(Twist, '/stretch/cmd_vel', 1) #/stretch_diff_drive_controller/cmd_vel for gazebo - self.subscriber_ = node.create_subscription(LaserScan, '/scan', self.set_speed, 10) +def main(args=None): + rclpy.init(args=args) + + avoider = Avoider() - # Give control to ROS. This will allow the callback to be called whenever new - # messages come in. If we don't put this line in, then the node will not work, - # and ROS will not process any messages. - rclpy.spin(node) + rclpy.spin(avoider) + + avoider.destroy_node() + rclpy.shutdown() - self.destroy_node() - rclpy.shutdown() if __name__ == '__main__': - # Setup Avoider object - avoider = Avoider() - avoider.main() + main() diff --git a/src/forward_kinematics.py b/stretch_ros_tutorials/forward_kinematics.py similarity index 100% rename from src/forward_kinematics.py rename to stretch_ros_tutorials/forward_kinematics.py diff --git a/stretch_ros_tutorials/marker.py b/stretch_ros_tutorials/marker.py new file mode 100755 index 0000000..5b92924 --- /dev/null +++ b/stretch_ros_tutorials/marker.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +# This node will publish a spherical marker above the current robot position, as +# it drives around the world + +import rclpy +from rclpy.node import Node + +# Import the Marker message type from the visualization_msgs package +from visualization_msgs.msg import Marker + +class Balloon(Node): + def __init__(self): + # Initialize the node named marker + super().__init__('stretch_marker') + + # Set up a publisher that will publish on a topic called balloon + self.publisher_ = self.create_publisher(Marker, 'balloon', 10) + + # Markers of all shapes share a common type + self.marker = Marker() + + # The frame ID is the frame in which the position of the marker is specified + # Refer to the wiki page for more marker types + self.marker.header.frame_id = '/base_link' + self.marker.header.stamp = self.get_clock().now().to_msg() + self.marker.type = self.marker.SPHERE + + # If you have more than one marker that you want displayed at a given time, + # then each needs to have a unique ID number + self.marker.id = 0 + + # You can Add, Delete, or Modify markers by defining the action + self.marker.action = self.marker.ADD + + # Define the size parameters for the marker + self.marker.scale.x = 0.5 + self.marker.scale.y = 0.5 + self.marker.scale.z = 0.5 + + # Define the color as an RGB triple from 0 to 1 + self.marker.color.r = 1.0 + self.marker.color.g = 0.0 + self.marker.color.b = 0.0 + + # Define the Alpha value, from 0 (invisible) to 1 (opaque) + self.marker.color.a = 1.0 + + # Specify the pose of the marker. Since spheres are rotationally invariant, + # we're only going to specify the positional elements. These are + # in the coordinate frame named in frame_id. In this case, the position + # will always be directly above the robot, and will move with it + self.marker.pose.position.x = 0.0 + self.marker.pose.position.y = 0.0 + self.marker.pose.position.z = 2.0 + + self.get_logger().info("Publishing the balloon topic. Use RViz to visualize.") + + def publish_marker(self): + # publisher the marker + self.publisher_.publish(self.marker) + +def main(args=None): + rclpy.init(args=args) + + balloon = Balloon() + + # This will loop until ROS shuts down the node. This can be done on the + # command line with a ctrl-C, or automatically from roslaunch. + while rclpy.ok(): + balloon.publish_marker() + + balloon.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/stretch_ros_tutorials/move.py b/stretch_ros_tutorials/move.py new file mode 100755 index 0000000..f36004c --- /dev/null +++ b/stretch_ros_tutorials/move.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node + +# Imports the built-in Twist message type that the node uses to structure the velocity data +from geometry_msgs.msg import Twist + + +class Move(Node): + def __init__(self): + super().__init__('stretch_base_move') + + # Setup a publisher that will send the velocity commands to the base + # This will publish on a topic called "/stretch/cmd_vel" with a message type Twist + self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10) + + self.get_logger().info("Starting to move in circle...") + + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.move_around) + + def move_around(self): + # Define a Twist message + command = Twist() + + # A Twist has three linear velocities (in meters per second), along each of the axes + # Stretch has a ddifferential drive base and can't move laterally, it will only pay + # attention to velocity along the x-axis + command.linear.x = 0.0 + command.linear.y = 0.0 + command.linear.z = 0.0 + + # A Twist also has three rotational velocities (in radians per second) + # Stretch will only respond to rotations along the z-axis + command.angular.x = 0.0 + command.angular.y = 0.0 + command.angular.z = 0.5 + + # Publish the Twist message + self.publisher_.publish(command) + + +def main(args=None): + rclpy.init(args=args) + + base_motion = Move() + + rclpy.spin(base_motion) + + base_motion.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + diff --git a/stretch_ros_tutorials/multipoint_command.py b/stretch_ros_tutorials/multipoint_command.py new file mode 100644 index 0000000..3fcba4f --- /dev/null +++ b/stretch_ros_tutorials/multipoint_command.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 + +# Every python controller needs these lines +import sys +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.duration import Duration + +# Import the FollowJointTrajectoryGoal from the control_msgs.action package to +# control the Stretch robot. +from control_msgs.action import FollowJointTrajectory + +# Import JointTrajectoryPoint from the trajectory_msgs package to define +# robot trajectories. +from trajectory_msgs.msg import JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +class MultiPointCommand(Node): + # Initialize the inhereted hm.Hellonode class. + def __init__(self): + super().__init__('stretch_multipoint_command') + + # Create an action client that can communicate with the /stretch_controller/follow_joint_trajectory action server + self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory') + server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0) + if not server_reached: + self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.') + sys.exit() + + # Create a subscriber for the /stretch/joint_states topic + self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1) + self.subscription + + self.get_logger().info('issuing multipoint command...') + + # Callback function for the /stretch/joint_states topic + def joint_states_callback(self, joint_state): + self.joint_state = joint_state + + def issue_multipoint_command(self): + joint_state = self.joint_state + duration0 = Duration(seconds=0.0) + duration1 = Duration(seconds=2.0) + duration2 = Duration(seconds=4.0) + duration3 = Duration(seconds=6.0) + duration4 = Duration(seconds=8.0) + duration5 = Duration(seconds=10.0) + + # Provide desired positions of lift, wrist extension, and yaw of + # the wrist + joint_value1 = joint_state.position[1] # joint_lift is at index 1 + joint_value2 = joint_state.position[0] # wrist_extension is at index 0 + joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8 + + # Set point0 as a JointTrajectoryPoint(). + point0 = JointTrajectoryPoint() + + # Provide desired positions of lift, wrist extension, and yaw of + # the wrist (in meters). + point0.positions = [joint_value1, joint_value2, joint_value3] + + # Provide desired velocity of the lift (m/s), wrist extension (m/s), + # and wrist yaw (rad/s). + # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s! + point0.velocities = [0.2, 0.2, 2.5] + + # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), + # and wrist yaw (rad/s^2). + point0.accelerations = [1.0, 1.0, 3.5] + + point0.time_from_start = duration0.to_msg() + + # Set positions for the following 5 trajectory points. + # IMPORTANT NOTE: If you do not provide any velocities or accelerations for the lift + # or wrist extension, then they go to their default values. However, the + # Velocity and Acceleration of the wrist yaw will stay the same from the + # previous value unless updated. + point1 = JointTrajectoryPoint() + point1.positions = [0.3, 0.1, 2.0] + point1.time_from_start = duration1.to_msg() + + point2 = JointTrajectoryPoint() + point2.positions = [0.5, 0.2, -1.0] + point2.time_from_start = duration2.to_msg() + + point3 = JointTrajectoryPoint() + point3.positions = [0.6, 0.3, 0.0] + point3.time_from_start = duration3.to_msg() + + point4 = JointTrajectoryPoint() + point4.positions = [0.8, 0.2, 1.0] + point4.time_from_start = duration4.to_msg() + + point5 = JointTrajectoryPoint() + point5.positions = [0.5, 0.1, 0.0] + point5.time_from_start = duration5.to_msg() + + # Set trajectory_goal as a FollowJointTrajectoryGoal and define + # the joint names as a list. + trajectory_goal = FollowJointTrajectory.Goal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] + + # Then trajectory_goal.trajectory.points is defined by a list of the joint + # trajectory points + trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] + + # Specify the coordinate frame that we want (base_link) and set the time to be now. + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory_goal.trajectory.header.frame_id = 'base_link' + + # Make the action call and send the goal. The last line of code waits + # for the result before it exits the python script. + self.trajectory_client.send_goal_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) + +def main(args=None): + rclpy.init(args=args) + + multipoint_command = MultiPointCommand() + + rclpy.spin_once(multipoint_command) + multipoint_command.issue_multipoint_command() + + rclpy.spin(multipoint_command) + + multipoint_command.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/stretch_ros_tutorials/scan_filter.py b/stretch_ros_tutorials/scan_filter.py new file mode 100755 index 0000000..868a0e4 --- /dev/null +++ b/stretch_ros_tutorials/scan_filter.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +# Every python controller needs these lines +import rclpy +from rclpy.node import Node +from numpy import linspace, inf +from math import sin + +# We're going to subscribe to a LaserScan message +from sensor_msgs.msg import LaserScan + + +class ScanFilter(Node): + def __init__(self): + # Initialize a ROS node named scan_filter + super().__init__('stretch_scan_filter') + + # Set up a publisher that will publish on a topic called "filtered_scan", + # with a LaserScan message type + self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10) #/stretch_diff_drive_controller/cmd_vel for gazebo + + # Set up a subscriber. We're going to subscribe to the topic "scan", + # looking for LaserScan messages. When a message comes in, ROS is going + # to pass it to the function "callback" automatically + self.sub = self.create_subscription(LaserScan, '/scan', self.scan_filter_callback, 10) + + # We're going to assume that the robot is pointing up the x-axis, so that + # any points with y coordinates further than half of the defined + # width (1 meter) from the axis are not considered + self.width = 1 + self.extent = self.width / 2.0 + + self.get_logger().info("Publishing the filtered_scan topic. Use RViz to visualize.") + + def scan_filter_callback(self,msg): + # Figure out the angles of the scan. We're going to do this each time, in case we're subscribing to more than one + # laser, with different numbers of beams + angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) + + # Work out the y coordinates of the ranges + points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] + + # If we're close to the x axis, keep the range, otherwise use inf, which means "no return" + new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] + + # Substitute in the new ranges in the original message, and republish it + msg.ranges = new_ranges + self.pub.publish(msg) + + +def main(args=None): + # First the rclpy library is initialized + rclpy.init(args=args) + + # Create object of the ScanFilter class + scan_filter = ScanFilter() + + # Give control to ROS. This will allow the callback to be called whenever new + # messages come in. If we don't put this line in, then the node will not work, + # and ROS will not process any messages + rclpy.spin(scan_filter) + + scan_filter.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/stretch_ros_tutorials/stow_command.py b/stretch_ros_tutorials/stow_command.py new file mode 100755 index 0000000..ac89cf1 --- /dev/null +++ b/stretch_ros_tutorials/stow_command.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 + +# Every python controller needs these lines +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.action import ActionClient + +import sys + +# Import the FollowJointTrajectoryGoal from the control_msgs.action package to +# control the Stretch robot. +from control_msgs.action import FollowJointTrajectory + +# Import JointTrajectoryPoint from the trajectory_msgs package to define +# robot trajectories and JointState from sensor_msgs to store joint states +from trajectory_msgs.msg import JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +class StowCommand(Node): + def __init__(self): + # Initialize a ROS node named stow_command + super().__init__('stretch_stow_command') + self.joint_state = JointState() + + # Create an action client that can communicate with the /stretch_controller/follow_joint_trajectory action server + self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory') + server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0) + if not server_reached: + self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.') + sys.exit() + + # Create a subscriber for the /stretch/joint_states topic + self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1) + self.subscription + + # Callback function for the /stretch/joint_states topic + def joint_states_callback(self, joint_state): + self.joint_state = joint_state + + def issue_stow_command(self): + joint_state = self.joint_state + if (joint_state is not None): + self.get_logger().info('stowing...') + + # Set two points as JointTrajectoryPoint(): stow_point1 is the current state, + # stow_point2 is the goal state + stow_point1 = JointTrajectoryPoint() + stow_point2 = JointTrajectoryPoint() + duration1 = Duration(seconds=0.0) + duration2 = Duration(seconds=4.0) + stow_point1.time_from_start = duration1.to_msg() + stow_point2.time_from_start = duration2.to_msg() + + # Provide desired positions of lift, wrist extension, and yaw of + # the wrist + joint_value1 = joint_state.position[1] # joint_lift is at index 1 + joint_value2 = joint_state.position[0] # wrist_extension is at index 0 + joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8 + stow_point1.positions = [joint_value1, joint_value2, joint_value3] + stow_point2.positions = [0.2, 0.0, 3.14] + + # Set trajectory_goal as a FollowJointTrajectoryGoal and define + # the joint names as a list. + trajectory_goal = FollowJointTrajectory.Goal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] + + # Then trajectory_goal.trajectory.points is defined by the positions + # set in stow_point1 and stow_point2 + trajectory_goal.trajectory.points = [stow_point1, stow_point2] + + # Specify the coordinate frame that we want (base_link) and set the time to be now + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory_goal.trajectory.header.frame_id = 'base_link' + + # Make the action call and send the goal + self.trajectory_client.send_goal_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) + + +# Create a funcion, main(), to do all of the setup the hm.HelloNode class +# and issue the stow command. +def main(args=None): + rclpy.init(args=args) + + # Create object of the StowCommand class + stow_command = StowCommand() + + # spin_once allows the joint_states_callback functions to be executed once + # for joint states to be received to this node + rclpy.spin_once(stow_command) + stow_command.issue_stow_command() + + rclpy.spin(stow_command) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + stow_command.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/stretch_ros_tutorials/tf_broadcaster.py b/stretch_ros_tutorials/tf_broadcaster.py new file mode 100644 index 0000000..a955d2f --- /dev/null +++ b/stretch_ros_tutorials/tf_broadcaster.py @@ -0,0 +1,88 @@ +from geometry_msgs.msg import TransformStamped + +import rclpy +from rclpy.node import Node +from tf2_ros import TransformBroadcaster +import tf_transformations + + +# This node publishes three child static frames in reference to their parent frames as below: +# parent -> link_mast child -> fk_link_mast +# parent -> link_lift child -> fk_link_lift +# parent -> link_wrist_yaw child -> fk_link_wrist_yaw +# Tf frames are not defined according to the DH convention by default +# The new frames have been declared to make the computation of +# forward and inverse kinematics easier by defining the new frames +# according to the DH convention +class FixedFrameBroadcaster(Node): + def __init__(self): + super().__init__('stretch_tf_broadcaster') + self.br = TransformBroadcaster(self) + time_period = 0.1 # seconds + self.timer = self.create_timer(time_period, self.broadcast_timer_callback) + + self.mast = TransformStamped() + self.mast.header.frame_id = 'link_mast' + self.mast.child_frame_id = 'fk_link_mast' + self.mast.transform.translation.x = 0.0 + self.mast.transform.translation.y = 0.0 + self.mast.transform.translation.z = 0.0 + q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) + self.mast.transform.rotation.x = q[0] + self.mast.transform.rotation.y = q[1] + self.mast.transform.rotation.z = q[2] + self.mast.transform.rotation.w = q[3] + + self.lift = TransformStamped() + self.lift.header.stamp = self.get_clock().now().to_msg() + self.lift.header.frame_id = 'link_lift' + self.lift.child_frame_id = 'fk_link_lift' + self.lift.transform.translation.x = 0.0 + self.lift.transform.translation.y = 2.0 + self.lift.transform.translation.z = 0.0 + q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) + self.lift.transform.rotation.x = q[0] + self.lift.transform.rotation.y = q[1] + self.lift.transform.rotation.z = q[2] + self.lift.transform.rotation.w = q[3] + self.br.sendTransform(self.lift) + + self.wrist = TransformStamped() + self.wrist.header.stamp = self.get_clock().now().to_msg() + self.wrist.header.frame_id = 'link_wrist_yaw' + self.wrist.child_frame_id = 'fk_link_wrist_yaw' + self.wrist.transform.translation.x = 0.0 + self.wrist.transform.translation.y = 2.0 + self.wrist.transform.translation.z = 0.0 + q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707) + self.wrist.transform.rotation.x = q[0] + self.wrist.transform.rotation.y = q[1] + self.wrist.transform.rotation.z = q[2] + self.wrist.transform.rotation.w = q[3] + self.br.sendTransform(self.wrist) + + self.get_logger().info("Publishing Tf frames. Use RViz to visualize.") + + def broadcast_timer_callback(self): + self.mast.header.stamp = self.get_clock().now().to_msg() + self.br.sendTransform(self.mast) + + self.lift.header.stamp = self.get_clock().now().to_msg() + self.br.sendTransform(self.lift) + + self.wrist.header.stamp = self.get_clock().now().to_msg() + self.br.sendTransform(self.wrist) + + +def main(args=None): + rclpy.init(args=args) + tf_broadcaster = FixedFrameBroadcaster() + + rclpy.spin(tf_broadcaster) + + tf_broadcaster.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/tf_listener.py b/stretch_ros_tutorials/tf_listener.py similarity index 82% rename from src/tf_listener.py rename to stretch_ros_tutorials/tf_listener.py index 511e2f0..661d173 100644 --- a/src/tf_listener.py +++ b/stretch_ros_tutorials/tf_listener.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +from rclpy.time import Time from tf2_ros import TransformException from tf2_ros.buffer import Buffer @@ -18,11 +19,13 @@ class FrameListener(Node): self.target_frame = self.get_parameter( 'target_frame').get_parameter_value().string_value + # Start a Tf buffer that will store the tf information for a few seconds self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) # Call on_timer function every second - self.timer = self.create_timer(1.0, self.on_timer) + time_period = 1.0 # seconds + self.timer = self.create_timer(time_period, self.on_timer) def on_timer(self): # Store frame names in variables that will be used to @@ -30,9 +33,9 @@ class FrameListener(Node): from_frame_rel = self.target_frame to_frame_rel = 'fk_link_mast' - # Look up for the transformation between target_frame and link_mast frames + # Look up the transformation between target_frame and link_mast frames try: - now = rclpy.time.Time() + now = Time() trans = self.tf_buffer.lookup_transform( to_frame_rel, from_frame_rel, @@ -53,5 +56,8 @@ def main(): rclpy.spin(node) except KeyboardInterrupt: pass + rclpy.shutdown() - rclpy.shutdown() \ No newline at end of file + +if __name__ == '__main__': + main() \ No newline at end of file From c1fee37bacf2125c2f15707e64c113c4511eeb61 Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Thu, 1 Sep 2022 14:26:45 -0400 Subject: [PATCH 10/10] Update the running instructions: first pass --- README.md | 6 +- example_1.md | 115 ++++++------ example_2.md | 73 +++++--- example_3.md | 78 ++++---- example_4.md | 88 +++++---- follow_joint_trajectory.md | 340 +++++++++++++++++++++-------------- gazebo_basics.md | 5 + getting_started.md | 48 ++--- internal_state_of_stretch.md | 30 +--- moveit_basics.md | 14 +- navigation_stack.md | 5 + rviz_basics.md | 10 +- teleoperating_stretch.md | 6 +- 13 files changed, 468 insertions(+), 350 deletions(-) diff --git a/README.md b/README.md index 1aeb7da..f87fe68 100644 --- a/README.md +++ b/README.md @@ -11,15 +11,15 @@ This branch provides instructions on the installation and use of code on the Str 6. [Navigation Stack](navigation_stack.md) 7. [MoveIt! Basics](moveit_basics.md) 8. [Follow Joint Trajectory Commands](follow_joint_trajectory.md) -9. [FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap) + -## Other ROS Examples +## Other ROS 2 Examples To help you get started on your software development, here are examples of nodes to have the stretch perform simple tasks. 1. [Teleoperate Stretch with a Node](example_1.md) - Use a python script that sends velocity commands. diff --git a/example_1.md b/example_1.md index 5354d03..8a2f24c 100644 --- a/example_1.md +++ b/example_1.md @@ -1,56 +1,66 @@ ## Example 1 +

The goal of this example is to give you an enhanced understanding of how to control the mobile base by sending `Twist` messages to a Stretch robot. -Begin by running `roscore` in a terminal. Then set the ros parameter to *navigation* mode by running the following commands in a new terminal. - ```bash -rosparam set /stretch_driver/mode "navigation" -roslaunch stretch_core stretch_driver.launch +ros2 launch stretch_core stretch_driver.launch.py ``` -To drive the robot forward with the move node, type the following in a new terminal. +To drive the robot in circles with the move node, type the following in a new terminal. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 move.py +ros2 run stetch_ros_tutorials move ``` To stop the node from sending twist messages, type **Ctrl** + **c**. ### The Code -Below is the code which will send *Twist* messages to drive the robot forward. +Below is the code which will send *Twist* messages to drive the robot in circles. ```python -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from geometry_msgs.msg import Twist -class Move: +class Move(Node): def __init__(self): - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo + super().__init__('stretch_base_move') + self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10) + + self.get_logger().info("Starting to move in circle...") + + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.move_around) - def move_forward(self): + def move_around(self): command = Twist() - command.linear.x = 0.1 + command.linear.x = 0.0 command.linear.y = 0.0 command.linear.z = 0.0 + command.angular.x = 0.0 command.angular.y = 0.0 - command.angular.z = 0.0 - self.pub.publish(command) + command.angular.z = 0.5 -if __name__ == '__main__': - rospy.init_node('move') + self.publisher_.publish(command) + + +def main(args=None): + rclpy.init(args=args) base_motion = Move() - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - base_motion.move_forward() - rate.sleep() + rclpy.spin(base_motion) + base_motion.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() ``` ### The Code Explained @@ -58,25 +68,32 @@ if __name__ == '__main__': Now let's break the code down. ```python -#!/usr/bin/env python +#!/usr/bin/env python3 ``` 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. ```python -import rospy +import rclpy +from rclpy.node import Node from geometry_msgs.msg import Twist ``` -You need to import rospy if you are writing a ROS Node. The geometry_msgs.msg import is so that we can send velocity commands to the robot. +You need to import rclpy if you are writing a ROS 2 Node. The geometry_msgs.msg import is so that we can send velocity commands to the robot. ```python -class Move: +class Move(Node): def __init__(self): - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo + super().__init__('stretch_base_move') + self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10) ``` -This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist. The queue_size argument limits the amount of queued messages if any subscriber is not receiving them fast enough. +This section of code defines the talker's interface to the rest of ROS. self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 10) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist. The queue_size argument limits the amount of queued messages if any subscriber is not receiving them fast enough. +```Python + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.move_around) +``` +We create a timer with a period of 0.5 seconds. This timer ensures that the function move_around is called every 0.5 seconds. This ensures a constant rate of 2Hz for the execution loop. ```Python command = Twist() @@ -85,45 +102,39 @@ Make a Twist message. We're going to set all of the elements, since we can't depend on them defaulting to safe values. ```python -command.linear.x = 0.1 -command.linear.y = 0.0 -command.linear.z = 0.0 + command.linear.x = 0.0 + command.linear.y = 0.0 + command.linear.z = 0.0 ``` -A Twist has three linear velocities (in meters per second), along each of the axes. For Stretch, it will only pay attention to the x velocity, since it can't directly move in the y direction or the z direction. +A Twist has three linear velocities (in meters per second), along each of the axes. For Stretch, it will only pay attention to the x velocity, since it can't directly move in the y direction or the z direction. We set the linear velocities to 0. ```python -command.angular.x = 0.0 -command.angular.y = 0.0 -command.angular.z = 0.0 + command.angular.x = 0.0 + command.angular.y = 0.0 + command.angular.z = 0.5 ``` A *Twist* also has three rotational velocities (in radians per second). -The Stretch will only respond to rotations around the z (vertical) axis. +The Stretch will only respond to rotations around the z (vertical) axis. We set this to a non-zero value. ```python -self.pub.publish(command) +self.publisher_.publish(command) ``` Publish the Twist commands in the previously defined topic name */stretch/cmd_vel*. ```Python -rospy.init_node('move') -base_motion = Move() -rate = rospy.Rate(10) -``` -The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". - -The `rospy.Rate()` function creates a Rate object rate. With the help of its method sleep(), it offers a convenient way for looping at the desired rate. With its argument of 10, we should expect to go through the loop 10 times per second (as long as our processing time does not exceed 1/10th of a second!) - -```python -while not rospy.is_shutdown(): - base_motion.move_forward() - rate.sleep() +def main(args=None): + rclpy.init(args=args) + base_motion = Move() + rclpy.spin(base_motion) + base_motion.destroy_node() + rclpy.shutdown() ``` -This loop is a fairly standard rospy construct: checking the rospy.is_shutdown() flag and then doing work. You have to check is_shutdown() to check if your program should exit (e.g. if there is a Ctrl-C or otherwise). The loop calls rate.sleep(), which sleeps just long enough to maintain the desired rate through the loop. +The next line, rclpy.init(args=args), is very important as it tells ROS to initialize the node. Until rclpy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name 'stretch_base_move'. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". We then create an instance called base_motion of the class Move(). This is then spun using the spin function in rclpy to call the callback functions, in our case the timer that ensures the move_around function is called at a steady rate of 2Hz. -## Move Stretch in Simulation + To stop the node from sending twist messages, type **Ctrl** + **c**. diff --git a/example_2.md b/example_2.md index 5102a1e..9269f18 100644 --- a/example_2.md +++ b/example_2.md @@ -2,6 +2,7 @@ The aim of this example is to provide instruction on how to filter scan messages. + For robots with laser scanners, ROS provides a special Message type in the [sensor_msgs](http://wiki.ros.org/sensor_msgs) package called [LaserScan](http://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html) to hold information about a given scan. Let's take a look at the message specification itself: ``` @@ -43,24 +44,23 @@ Knowing the orientation of the LiDAR allows us to filter the scan values for a d First, open a terminal and run the stretch driver launch file. ```bash -roslaunch stretch_core stretch_driver.launch +ros2 launch stretch_core stretch_driver.launch.py ``` Then in a new terminal run the rplidar launch file from `stretch_core`. ```bash -roslaunch stretch_core rplidar.launch +ros2 launch stretch_core rplidar.launch.py ``` To filter the lidar scans for ranges that are directly in front of Stretch (width of 1 meter) run the scan filter node by typing the following in a new terminal. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 scan_filter.py +ros2 run stretch_ros_tutorials scan_filter ``` Then run the following command to bring up a simple RViz configuration of the Stretch robot. ```bash -rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz +ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibration`/rviz/stretch_simple_test.rviz ``` Change the topic name from the LaserScan display from */scan* to */filter_scan*. @@ -71,31 +71,47 @@ Change the topic name from the LaserScan display from */scan* to */filter_scan*. ### The Code ```python -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from numpy import linspace, inf from math import sin from sensor_msgs.msg import LaserScan -class Scanfilter: +class ScanFilter(Node): def __init__(self): - self.width = 1.0 + super().__init__('stretch_scan_filter') + self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10) + + self.sub = self.create_subscription(LaserScan, '/scan', self.scan_filter_callback, 10) + + self.width = 1 self.extent = self.width / 2.0 - self.sub = rospy.Subscriber('/scan', LaserScan, self.callback) - self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10) - def callback(self,msg): + self.get_logger().info("Publishing the filtered_scan topic. Use RViz to visualize.") + + def scan_filter_callback(self,msg): angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] msg.ranges = new_ranges self.pub.publish(msg) + +def main(args=None): + rclpy.init(args=args) + + scan_filter = ScanFilter() + + rclpy.spin(scan_filter) + + scan_filter.destroy_node() + rclpy.shutdown() + + if __name__ == '__main__': - rospy.init_node('scan_filter') - Scanfilter() - rospy.spin() + main() ``` ### The Code Explained @@ -103,18 +119,19 @@ if __name__ == '__main__': Now let's break the code down. ```python -#!/usr/bin/env python +#!/usr/bin/env python3 ``` 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. ```python -import rospy +import rclpy +from rclpy.node import Node from numpy import linspace, inf from math import sin from sensor_msgs.msg import LaserScan ``` -You need to import rospy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, thus why linspace, inf, and sin are imported. The sensor_msgs.msg import is so that we can subscribe and publish LaserScan messages. +You need to import rclpy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, that's why linspace, inf, and sin are imported. The sensor_msgs.msg import is so that we can subscribe and publish LaserScan messages. ```python self.width = 1 @@ -123,14 +140,14 @@ self.extent = self.width / 2.0 We're going to assume that the robot is pointing up the x-axis, so that any points with y coordinates further than half of the defined width (1 meter) from the axis are not considered. ```python -self.sub = rospy.Subscriber('/scan', LaserScan, self.callback) +self.sub = self.create_subscription(LaserScan, '/scan', self.scan_filter_callback, 10) ``` -Set up a subscriber. We're going to subscribe to the topic "scan", looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "callback" automatically. +Set up a subscriber. We're going to subscribe to the topic "/scan", looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "callback" automatically. ```python -self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10) +self.pub = self.create_publisher(LaserScan, '/filtered_scan', 10) ``` -`pub = rospy.Publisher("filtered_scan", LaserScan, queue_size=10)` declares that your node is publishing to the *filtered_scan* topic using the message type LaserScan. This lets the master tell any nodes listening on *filtered_scan* that we are going to publish data on that topic. +This declares that your node is publishing to the *filtered_scan* topic using the message type LaserScan. This lets any nodes listening on *filtered_scan* that we are going to publish data on that topic. ```python angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) @@ -155,15 +172,17 @@ self.pub.publish(msg) Substitute in the new ranges in the original message, and republish it. ```python -rospy.init_node('scan_filter') -Scanfilter() +def main(args=None): + rclpy.init(args=args) + + scan_filter = ScanFilter() ``` -The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". +The next line, rclpy.init_node initializes the node. In this case, your node will take on the name 'stretch_scan_filter'. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". -Setup Scanfilter class with `Scanfilter()` +Setup Scanfilter class with `scan_filter = Scanfilter()` ```python -rospy.spin() +rclpy.spin(scan_filter) ``` Give control to ROS. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, diff --git a/example_3.md b/example_3.md index 3dd757e..cc61c6b 100644 --- a/example_3.md +++ b/example_3.md @@ -2,20 +2,16 @@ The aim of example 3 is to combine the two previous examples and have Stretch utilize its laser scan data to avoid collision with objects as it drives forward. -Begin by running `roscore` in a terminal. Then set the ros parameter to *navigation* mode by running the following commands in a new terminal. - ```bash -rosparam set /stretch_driver/mode "navigation" -roslaunch stretch_core stretch_driver.launch +ros2 launch stretch_core stretch_driver.launch.py ``` Then in a new terminal type the following to activate the LiDAR sensor. ```bash -roslaunch stretch_core rplidar.launch +ros2 launch stretch_core rplidar.launch.py ``` To activate the avoider node, type the following in a new terminal. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 avoider.py +ros2 run stretch_ros_tutorials avoider ``` To stop the node from sending twist messages, type **Ctrl** + **c** in the terminal running the avoider node. @@ -26,21 +22,22 @@ To stop the node from sending twist messages, type **Ctrl** + **c** in the termi ### The Code ```python -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from numpy import linspace, inf, tanh from math import sin from geometry_msgs.msg import Twist from sensor_msgs.msg import LaserScan -class Avoider: - def __init__(self): - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo - self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed) +class Avoider(Node): + def __init__(self): + super().__init__('stretch_avoider') self.width = 1 self.extent = self.width / 2.0 + self.distance = 0.5 self.twist = Twist() @@ -51,19 +48,32 @@ class Avoider: self.twist.angular.y = 0.0 self.twist.angular.z = 0.0 - def set_speed(self,msg): + self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1) + self.subscriber_ = self.create_subscription(LaserScan, '/scan', self.set_speed, 10) + + def set_speed(self, msg): angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) + points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] + new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] + error = min(new_ranges) - self.distance self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0 - self.pub.publish(self.twist) + self.publisher_.publish(self.twist) + + +def main(args=None): + rclpy.init(args=args) + avoider = Avoider() + rclpy.spin(avoider) + avoider.destroy_node() + rclpy.shutdown() + if __name__ == '__main__': - rospy.init_node('avoider') - Avoider() - rospy.spin() + main() ``` ### The Code Explained @@ -71,31 +81,32 @@ if __name__ == '__main__': Now let's break the code down. ```python -#!/usr/bin/env python +#!/usr/bin/env python3 ``` 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. ```python -import rospy +import rclpy +from rclpy.node import Node from numpy import linspace, inf, tanh from math import sin from geometry_msgs.msg import Twist from sensor_msgs.msg import LaserScan ``` -You need to import rospy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, thus linspace, inf, tanh, and sin are imported. The sensor_msgs.msg import is so that we can subscribe to LaserScan messages. The geometry_msgs.msg import is so that we can send velocity commands to the robot. +You need to import rclpy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, thus linspace, inf, tanh, and sin are imported. The sensor_msgs.msg import is so that we can subscribe to LaserScan messages. The geometry_msgs.msg import is so that we can send velocity commands to the robot. ```python -self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo +self.publisher_ = self.create_publisher(Twist, '/stretch/cmd_vel', 1) ``` -This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist. +This declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist. ```python -self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed) +self.subscriber_ = self.create_subscription(LaserScan, '/scan', self.set_speed, 10) ``` -Set up a subscriber. We're going to subscribe to the topic "scan", looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "set_speed" automatically. +Set up a subscriber. We're going to subscribe to the topic "/scan", looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "set_speed" automatically. ```python @@ -138,19 +149,22 @@ Set the speed according to a tanh function. This method gives a nice smooth mapp ```python -self.pub.publish(self.twist) +self.publisher_.publish(self.twist) ``` Publish the Twist message. ```python -rospy.init_node('avoider') -Avoider() -rospy.spin() +def main(args=None): + rclpy.init(args=args) + avoider = Avoider() + rclpy.spin(avoider) + avoider.destroy_node() + rclpy.shutdown() ``` -The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". +The next line, rclpy.init() method initializes the node. In this case, your node will take on the name 'stretch_avoider'. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". -Setup Avoider class with `Avioder()` +Setup Avoider class with `avoider = Avioder()` -Give control to ROS with `rospy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages. +Give control to ROS with `rclpy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages. **Next Example:** [Example 4](example_4.md) diff --git a/example_4.md b/example_4.md index 53349c3..81e08ee 100644 --- a/example_4.md +++ b/example_4.md @@ -2,16 +2,16 @@ ![image](images/balloon.png) -Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command. +Let's bringup stretch in RViz by using the following command. ```bash -roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true +ros2 launch stretch_core stretch_driver.launch.py +ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibrtion`/rviz/stretch_simple_test.rviz ``` -the `rviz` flag will open an RViz window to visualize a variety of ROS topics. In a new terminal run the following commands to create a marker. +In a new terminal run the following commands to create a marker. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 marker.py +ros2 run stretch_ros_tutorials marker ``` The gif below demonstrates how to add a new *Marker* display type, and change the topic name from `visualization_marker` to `balloon`. A red sphere Marker should appear above the Stretch robot. @@ -20,43 +20,57 @@ The gif below demonstrates how to add a new *Marker* display type, and change th ### The Code ```python -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from visualization_msgs.msg import Marker -class Balloon(): +class Balloon(Node): def __init__(self): - self.publisher = rospy.Publisher('balloon', Marker, queue_size=10) + super().__init__('stretch_marker') + + self.publisher_ = self.create_publisher(Marker, 'balloon', 10) + self.marker = Marker() + self.marker.header.frame_id = '/base_link' - self.marker.header.stamp = rospy.Time() + self.marker.header.stamp = self.get_clock().now().to_msg() self.marker.type = self.marker.SPHERE + self.marker.id = 0 + self.marker.action = self.marker.ADD + self.marker.scale.x = 0.5 self.marker.scale.y = 0.5 self.marker.scale.z = 0.5 + self.marker.color.r = 1.0 self.marker.color.g = 0.0 self.marker.color.b = 0.0 + self.marker.color.a = 1.0 + self.marker.pose.position.x = 0.0 self.marker.pose.position.y = 0.0 self.marker.pose.position.z = 2.0 + self.get_logger().info("Publishing the balloon topic. Use RViz to visualize.") + def publish_marker(self): - self.publisher.publish(self.marker) + self.publisher_.publish(self.marker) +def main(args=None): + rclpy.init(args=args) + balloon = Balloon() + while rclpy.ok(): + balloon.publish_marker() + balloon.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - rospy.init_node('marker', argv=sys.argv) - ballon = Balloon() - rate = rospy.Rate(10) - - while not rospy.is_shutdown(): - ballon.publish_marker() - rate.sleep() + main() ``` @@ -64,30 +78,33 @@ if __name__ == '__main__': Now let's break the code down. ```python -#!/usr/bin/env python +#!/usr/bin/env python3 ``` 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. ```python -import rospy +import rclpy +from rclpy.node import Node from visualization_msgs.msg import Marker ``` -You need to import rospy if you are writing a ROS Node. Import the `Marker` type from the visualization_msgs.msg package. This import is required to publish a Marker, which will be visualized in RViz. +You need to import rclpy if you are writing a ROS 2 Node. Import the `Marker` type from the visualization_msgs.msg package. This import is required to publish a Marker, which will be visualized in RViz. ```python -self.pub = rospy.Publisher('balloon', Marker, queue_size=10) +self.publisher_ = self.create_publisher(Marker, 'balloon', 10) ``` -This section of code defines the talker's interface to the rest of ROS. pub = rospy.Publisher("balloon", Twist, queue_size=1) declares that your node is publishing to the */ballon* topic using the message type *Twist*. +This declares that your node is publishing to the */ballon* topic using the message type *Twist*. ```python self.marker = Marker() self.marker.header.frame_id = '/base_link' -self.marker.header.stamp = rospy.Time() +self.marker.header.stamp = self.get_clock().now().to_msg() self.marker.type = self.marker.SPHERE + ``` + Create a maker. Markers of all shapes share a common type. Set the frame ID and type. The frame ID is the frame in which the position of the marker is specified. The type is the shape of the marker. Further details on marker shapes can be found here: [RViz Markers](http://wiki.ros.org/rviz/DisplayTypes/Marker) ```python @@ -130,27 +147,26 @@ Specify the pose of the marker. Since spheres are rotationally invariant, we're ```python def publish_marker(self): - self.publisher.publish(self.marker) + self.publisher_.publish(self.marker) ``` Publish the Marker data structure to be visualized in RViz. ```python -rospy.init_node('marker', argv=sys.argv) -ballon = Balloon() -rate = rospy.Rate(10) +def main(args=None): + rclpy.init(args=args) + balloon = Balloon() ``` -The next line, rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". - -Setup Balloon class with `Balloon()` +The next line, rospy.init. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/". -Give control to ROS with `rospy.spin()`. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages. +Setup Balloon class with `balloon = Balloon()` ```python -while not rospy.is_shutdown(): - ballon.publish_marker() - rate.sleep() +while rclpy.ok(): + balloon.publish_marker() + balloon.destroy_node() + rclpy.shutdown() ``` -This loop is a fairly standard rospy construct: checking the rospy.is_shutdown() flag and then doing work. You have to check is_shutdown() to check if your program should exit (e.g. if there is a Ctrl-C or otherwise). The loop calls rate.sleep(), which sleeps just long enough to maintain the desired rate through the loop. +This loop is a fairly standard rclpy construct: checking the rclpy.ok() flag and then doing work. You have to run this check to see if your program should exit (e.g. if there is a Ctrl-C or otherwise). diff --git a/follow_joint_trajectory.md b/follow_joint_trajectory.md index 6a638ab..9b1693a 100644 --- a/follow_joint_trajectory.md +++ b/follow_joint_trajectory.md @@ -1,24 +1,22 @@ ## FollowJointTrajectory Commands -Stretch ROS driver offers a [`FollowJointTrajectory`](http://docs.ros.org/en/api/control_msgs/html/action/FollowJointTrajectory.html) action service for its arm. Within this tutorial we will have a simple FollowJointTrajectory command sent to a Stretch robot to execute. +Stretch driver offers a [`FollowJointTrajectory`](http://docs.ros.org/en/api/control_msgs/html/action/FollowJointTrajectory.html) action service for its arm. Within this tutorial we will have a simple FollowJointTrajectory command sent to a Stretch robot to execute. ## Stow Command Example

-Begin by running `roscore` in a terminal. Then set the ros parameter to *position* mode by running the following commands in a new terminal. +Begin by launching stretch_driver in a terminal. ```bash -rosparam set /stretch_driver/mode "position" -roslaunch stretch_core stretch_driver.launch +ros2 launch stretch_core stretch_driver.launch.py ``` In a new terminal type the following commands. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 stow_command.py +ros2 run stretch_ros_tutorials stow_command ``` This will send a FollowJointTrajectory command to stow Stretch's arm. @@ -27,45 +25,77 @@ This will send a FollowJointTrajectory command to stow Stretch's arm. ```python #!/usr/bin/env python3 -import rospy -from control_msgs.msg import FollowJointTrajectoryGoal +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.action import ActionClient +import sys +from control_msgs.action import FollowJointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm -import time - -class StowCommand(hm.HelloNode): +from sensor_msgs.msg import JointState +class StowCommand(Node): def __init__(self): - hm.HelloNode.__init__(self) + super().__init__('stretch_stow_command') + self.joint_state = JointState() + + self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory') + server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0) + if not server_reached: + self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.') + sys.exit() + + self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1) + self.subscription + + def joint_states_callback(self, joint_state): + self.joint_state = joint_state 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] + joint_state = self.joint_state + if (joint_state is not None): + self.get_logger().info('stowing...') - 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' + stow_point1 = JointTrajectoryPoint() + stow_point2 = JointTrajectoryPoint() + duration1 = Duration(seconds=0.0) + duration2 = Duration(seconds=4.0) + stow_point1.time_from_start = duration1.to_msg() + stow_point2.time_from_start = duration2.to_msg() + + joint_value1 = joint_state.position[1] # joint_lift is at index 1 + joint_value2 = joint_state.position[0] # wrist_extension is at index 0 + joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8 + stow_point1.positions = [joint_value1, joint_value2, joint_value3] + stow_point2.positions = [0.2, 0.0, 3.14] + + trajectory_goal = FollowJointTrajectory.Goal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] + + trajectory_goal.trajectory.points = [stow_point1, stow_point2] + + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory_goal.trajectory.header.frame_id = 'base_link' + + self.trajectory_client.send_goal_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) - 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(args=None): + rclpy.init(args=args) + + stow_command = StowCommand() + + rclpy.spin_once(stow_command) + stow_command.issue_stow_command() + rclpy.spin(stow_command) + + stow_command.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - node = StowCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + main() ``` ### The Code Explained @@ -79,65 +109,81 @@ Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at ```python -import rospy -from control_msgs.msg import FollowJointTrajectoryGoal +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.action import ActionClient +import sys +from control_msgs.action import FollowJointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm -import time +from sensor_msgs.msg import JointState ``` -You need to import rospy if you are writing a ROS Node. Import the FollowJointTrajectoryGoal from the [control_msgs.msg](http://wiki.ros.org/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](http://wiki.ros.org/trajectory_msgs) package to define robot trajectories. The [hello_helpers](https://github.com/hello-robot/stretch_ros/tree/master/hello_helpers) package consists of a module the 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. + +You need to import rclpy if you are writing a ROS 2 Node. Import the FollowJointTrajectory from the [control_msgs.msg](http://wiki.ros.org/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](http://wiki.ros.org/trajectory_msgs) package to define robot trajectories. ```python -class StowCommand(hm.HelloNode): - +class StowCommand(Node): def __init__(self): - hm.HelloNode.__init__(self) + super().__init__('stretch_stow_command') ``` -The `StowCommand ` class inherits the `HelloNode` class from `hm` and is initialized. +The `StowCommand ` class inherits from the `Node` class from and is initialized. ```python 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] ``` -The `issue_stow_command()` is the name of the function that will stow Stretch's arm. Within the function, we set *stow_point* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. These are defined in the next set of the code. +The `issue_stow_command()` method will stow Stretch's arm. Within the function, we set *stow_point* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. These are defined in the next set of the code. ```python - trajectory_goal = FollowJointTrajectoryGoal() + stow_point1 = JointTrajectoryPoint() + stow_point2 = JointTrajectoryPoint() + duration1 = Duration(seconds=0.0) + duration2 = Duration(seconds=4.0) + stow_point1.time_from_start = duration1.to_msg() + stow_point2.time_from_start = duration2.to_msg() + + joint_value1 = joint_state.position[1] # joint_lift is at index 1 + joint_value2 = joint_state.position[0] # wrist_extension is at index 0 + joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8 + stow_point1.positions = [joint_value1, joint_value2, joint_value3] + stow_point2.positions = [0.2, 0.0, 3.14] + + trajectory_goal = FollowJointTrajectory.Goal() 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.points = [stow_point1, stow_point2] + + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() trajectory_goal.trajectory.header.frame_id = 'base_link' ``` -Set *trajectory_goal* as a `FollowJointTrajectoryGoal` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point*. Specify the coordinate frame that we want (base_link) and set the time to be now. +Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point*. Specify the coordinate frame that we want (base_link) and set the time to be now. ```python -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_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) ``` -Make the action call and send the goal. The last line of code waits for the result before it exits the python script. +Make the action call and send the goal. ```python -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(args=None): + rclpy.init(args=args) + + stow_command = StowCommand() + + rclpy.spin_once(stow_command) + stow_command.issue_stow_command() + rclpy.spin(stow_command) + + stow_command.destroy_node() + rclpy.shutdown() ``` -Create a funcion, `main()`, to do all of the setup the `hm.HelloNode` class and issue the stow command. +Create a funcion, `main()`, to do all of the setup in the class and issue the stow command. Initialize the `StowCommand()` class and set it to *node* and run the `main()` function. ```python if __name__ == '__main__': - try: - node = StowCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + main() ``` -Initialize the `StowCommand()` class and set it to *node* and run the `main()` function. +To make the script executable call the main() function like above. ## Multipoint Command Example @@ -146,18 +192,16 @@ Initialize the `StowCommand()` class and set it to *node* and run the `main()` f

-Begin by running `roscore` in a terminal. Then set the ros parameter to *position* mode by running the following commands in a new terminal. +If you have killed the above instance of stretch_driver relaunch it again through the terminal. ```bash -rosparam set /stretch_driver/mode "position" -roslaunch stretch_core stretch_driver.launch +ros2 launch stretch_core stretch_driver.launch.py ``` In a new terminal type the following commands. ```bash -cd catkin_ws/src/stretch_ros_turotials/src/ -python3 multipoint_command.py +ros2 run stretch_ros_tutorials multipoint_command ``` This will send a list of JointTrajectoryPoint's to move Stretch's arm. @@ -166,61 +210,103 @@ This will send a list of JointTrajectoryPoint's to move Stretch's arm. ```python #!/usr/bin/env python3 -import rospy -import time -from control_msgs.msg import FollowJointTrajectoryGoal +import sys +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.duration import Duration +from control_msgs.action import FollowJointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm - -class MultiPointCommand(hm.HelloNode): +from sensor_msgs.msg import JointState +class MultiPointCommand(Node): def __init__(self): - hm.HelloNode.__init__(self) + super().__init__('stretch_multipoint_command') + + self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory') + server_reached = self.trajectory_client.wait_for_server(timeout_sec=60.0) + if not server_reached: + self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.') + sys.exit() + + self.subscription = self.create_subscription(JointState, '/stretch/joint_states', self.joint_states_callback, 1) + self.subscription + + self.get_logger().info('issuing multipoint command...') + + def joint_states_callback(self, joint_state): + self.joint_state = joint_state 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] + joint_state = self.joint_state + duration0 = Duration(seconds=0.0) + duration1 = Duration(seconds=2.0) + duration2 = Duration(seconds=4.0) + duration3 = Duration(seconds=6.0) + duration4 = Duration(seconds=8.0) + duration5 = Duration(seconds=10.0) + + joint_value1 = joint_state.position[1] # joint_lift is at index 1 + joint_value2 = joint_state.position[0] # wrist_extension is at index 0 + joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8 + + point0 = JointTrajectoryPoint() - point1 = JointTrajectoryPoint() - point1.positions = [0.3, 0.1, 2.0] + point0.positions = [joint_value1, joint_value2, joint_value3] - point2 = JointTrajectoryPoint() - point2.positions = [0.5, 0.2, -1.0] + point0.velocities = [0.2, 0.2, 2.5] - point3 = JointTrajectoryPoint() - point3.positions = [0.6, 0.3, 0.0] + point0.accelerations = [1.0, 1.0, 3.5] - point4 = JointTrajectoryPoint() - point4.positions = [0.8, 0.2, 1.0] + point0.time_from_start = duration0.to_msg() - point5 = JointTrajectoryPoint() - point5.positions = [0.5, 0.1, 0.0] + point1 = JointTrajectoryPoint() + point1.positions = [0.3, 0.1, 2.0] + point1.time_from_start = duration1.to_msg() - 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' + point2 = JointTrajectoryPoint() + point2.positions = [0.5, 0.2, -1.0] + point2.time_from_start = duration2.to_msg() - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() + point3 = JointTrajectoryPoint() + point3.positions = [0.6, 0.3, 0.0] + point3.time_from_start = duration3.to_msg() - 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) + point4 = JointTrajectoryPoint() + point4.positions = [0.8, 0.2, 1.0] + point4.time_from_start = duration4.to_msg() + + point5 = JointTrajectoryPoint() + point5.positions = [0.5, 0.1, 0.0] + point5.time_from_start = duration5.to_msg() + + trajectory_goal = FollowJointTrajectory.Goal() + 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 = self.get_clock().now().to_msg() + trajectory_goal.trajectory.header.frame_id = 'base_link' + + self.trajectory_client.send_goal_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) + +def main(args=None): + rclpy.init(args=args) + + multipoint_command = MultiPointCommand() + + rclpy.spin_once(multipoint_command) + multipoint_command.issue_multipoint_command() + + rclpy.spin(multipoint_command) + + multipoint_command.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - node = MultiPointCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') + main() ``` @@ -228,31 +314,25 @@ if __name__ == '__main__': Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the different components of the multipoint_command node. ```python -point0 = JointTrajectoryPoint() -point0.positions = [0.2, 0.0, 3.4] + point1 = JointTrajectoryPoint() + point1.positions = [0.3, 0.1, 2.0] + point1.time_from_start = duration1.to_msg() ``` -Sett *point0* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. - +Set *point1* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. +**IMPORTANT NOTE**: The lift and wrist extension can only go up to 0.2 m/s. If you do not provide any velocities or accelerations for the lift or wrist extension, then they go to their default values. However, the Velocity and Acceleration of the wrist yaw will stay the same from the previous value unless updated. -```python -point0.velocities = [0.2, 0.2, 2.5] -``` -Provide desired velocity of the lift (m/s), wrist extension (m/s), and wrist yaw (rad/s) for *point0*. ```python -point0.accelerations = [1.0, 1.0, 3.5] -``` -Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), and wrist yaw (rad/s^2). + trajectory_goal = FollowJointTrajectory.Goal() + trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] -**IMPORTANT NOTE**: The lift and wrist extension can only go up to 0.2 m/s. If you do not provide any velocities or accelerations for the lift or wrist extension, then they go to their default values. However, the Velocity and Acceleration of the wrist yaw will stay the same from the previous value unless updated. + trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] + trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory_goal.trajectory.header.frame_id = 'base_link' -```python -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_async(trajectory_goal) + self.get_logger().info('Sent stow goal = {0}'.format(trajectory_goal)) ``` -Set *trajectory_goal* as a `FollowJointTrajectoryGoal` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points. Specify the coordinate frame that we want (base_link) and set the time to be now. +Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points. Specify the coordinate frame that we want (base_link) and set the time to be now. diff --git a/gazebo_basics.md b/gazebo_basics.md index c9d75f4..a215561 100644 --- a/gazebo_basics.md +++ b/gazebo_basics.md @@ -1,5 +1,10 @@ # Spawning Stretch in Simulation (Gazebo) +### NOTE +Simulation support for Stretch in ROS 2 is under active development. Please reach out to us if you want to work with Stretch in a simulated environment like Gazebo/Ignition in ROS 2. + +Refer to the instructions below if you want to test this functionality in ROS 1. + ### Empty World Simulation To spawn the Stretch in gazebo's default empty world run the following command in your terminal. ```bash diff --git a/getting_started.md b/getting_started.md index 5594c7e..32c8556 100644 --- a/getting_started.md +++ b/getting_started.md @@ -1,51 +1,25 @@ # Getting Started -## Ubuntu - +## Installing Ubuntu 20.04 with ROS 2 Galactic on Stretch Hello Robot utilizes Ubuntu, an open source Linux operating system, for the Stretch RE1 platform. If you are unfamiliar with the operating system, we encourage you to review a [tutorial](https://ubuntu.com/tutorials/command-line-for-beginners#1-overview) provided by Ubuntu. Additionally, the Linux command line, BASH, is used to execute commands and is needed to run ROS on the Stretch robot. Here is a [tutorial](https://ryanstutorials.net/linuxtutorial/) on getting started with BASH. -## Installing Noetic on Stretch -Instructions on installing Noetic can be found in our open source [installation guide](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/install_noetic.md). Once your system is setup, clone the [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git) to your workspace. Then build the packages in your workspace. - -``` -cd ~/catkin_ws/src -git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git -cd ~/catkin_ws -catkin_make -``` - -## ROS Setup on Local Computer - -Hello Robot is currently running Stretch on Ubuntu 20.04 and on ROS Noetic. To begin the setup, begin with [installing Ubnutu desktop](https://ubuntu.com/tutorials/install-ubuntu-desktop#1-overview) on your local machine. Then follow the [installation guide for ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) on your system. + +Instructions on installing Ubuntu 20.04 with ROS Noetic and ROS 2 Galactic can be found in our open source [installation guide](https://github.com/hello-robot/stretch_ros/blob/dev/noetic/install_noetic.md). Following these steps should create a separate Ubuntu 20.04 partition with an ament worskspace created in the home directory. -Currently, the Realsense2_description package isn't installed by rosdep and requires a user to manually install the package. Run the following command in your terminal +## ROS 2 Tutorials Setup on Local Computer +Once your system is setup, clone the [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git) to the src directory of the ament workspace, then build the packages. -```bash -sudo apt-get install ros-noetic-realsense2-camera ``` - -After your system is setup, clone the [stretch_ros](https://github.com/hello-robot/stretch_ros.git), [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git), and [realsense_gazebo_plugin packages]( https://github.com/pal-robotics/realsense_gazebo_plugin) to your preferred workspace. Then install dependencies and build the packages. -```bash -cd ~/catkin_ws/src -git clone https://github.com/hello-robot/stretch_ros -git clone https://github.com/pal-robotics/realsense_gazebo_plugin +cd ~/ament_ws/src + git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git -cd ~/catkin_ws -rosdep install --from-paths src --ignore-src -r -y -catkin_make +cd ~/ament_ws +colcon build ``` - Then source your workspace with the following command -```bash -echo "source /home/USER_NAME/catkin_ws/devel/setup.bash" ``` - -## RoboMaker - -![image](images/aws-robomaker.png) - - -If a you are unable to dual boot and install ubuntu on your local machine, an alternative is to use [AWS RoboMaker](https://aws.amazon.com/robomaker/). AWS RoboMaker extends the ROS framework with cloud services. The service provides a robotics simulation service, allowing for testing of the Stretch RE1 platform. If you are a first-time user of AWS RoboMaker, follow the [guide here](https://github.com/aws-robotics/aws-robomaker-robotics-curriculum/blob/main/getting-started-with-aws-robomaker/_modules/mod-2a-aws.md) to get up and running with the service. +source ~/ament_ws/install/setup.bash" +``` **Next Tutorial:** [Gazebo Basics](gazebo_basics.md) diff --git a/internal_state_of_stretch.md b/internal_state_of_stretch.md index 99f2491..ed72b8c 100644 --- a/internal_state_of_stretch.md +++ b/internal_state_of_stretch.md @@ -2,16 +2,15 @@ Begin by starting up the stretch driver launch file by typing the following in a terminal. ```bash -roslaunch stretch_core stretch_driver.launch +ros2 launch stretch_core stretch_driver.launch.py ``` -Then utilize the ROS command-line tool, [rostopic](http://wiki.ros.org/rostopic), to display Stretch's internal state information. For instance, to view the current state of the robot's joints, simply type the following in a terminal. +Then utilize the ROS command-line tool, ros2 topic, to display Stretch's internal state information. For instance, to view the current state of the robot's joints, simply type the following in a terminal. ```bash -rostopic echo /joint_states -n1 +ros2 topic echo /stretch/joint_states ``` -Note that the flag, `-n1`, at the end of the command, defines the count of how many times you wish to publish the current topic information. If you prefer to continuously print the topic for debugging purposes, then remove the flag. -Your terminal will then output the information associated with the `/joint_states` topic. Your `header`, `position`, `velocity`, and `effort` information may vary from what is printed below. +Your terminal will then output the information associated with the `/stretch/joint_states` topic. Your `header`, `position`, `velocity`, and `effort` information may vary from what is printed below. ``` header: seq: 70999 @@ -28,27 +27,12 @@ effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] --- ``` -Let's say you are interested in only seeing the `header` component of the `/joint_states` topic, you can output this within the rostopic command-line tool by typing the following command. -```bash -rostopic echo /joint_states/header -n1 -``` -Your terminal will then output something similar to this: - -``` -seq: 97277 -stamp: - secs: 1945 - nsecs: 562000000 -frame_id: '' ---- -``` - -Additionally, if you were to type `rostopic echo /` in the terminal, then press your *Tab* key on your keyboard, you will see the list of active topics being published. +Additionally, if you type `ros2 topic list` in the terminal, you will see the list of active topics being published. -A powerful tool to visualize the ROS communication is the ROS [rqt_graph package](http://wiki.ros.org/rqt_graph). You can see a graph of topics being communicated between nodes by typing the following. +A powerful tool to visualize the ROS communication is through the rqt_graph package. You can see a graph of topics being communicated between nodes by typing the following. ``` -rqt_graph +ros2 run rqt_graph rqt_graph ``` ![image](images/rqt_graph.png) diff --git a/moveit_basics.md b/moveit_basics.md index 8ad2c22..6373124 100644 --- a/moveit_basics.md +++ b/moveit_basics.md @@ -14,17 +14,21 @@ roslaunch stretch_moveit_config moveit_rviz.launch ``` --> ## MoveIt! Without Hardware -To begin running MoveIt! on stretch, run the demo launch file. This doesn't require any simulator or robot to run. +To begin running MoveIt! on stretch, checkout to the feature/hybrid_planning branch and run the demo launch file. ```bash -roslaunch stretch_moveit_config demo.launch +cd ~/ament_ws/src/stretch_ros2 +git checkout feature/hybrid_planning +cd ~/ament_ws +colcon build +ros2 launch stretch_moveit_config movegroup_moveit2.launch.py ``` This will brining up an RViz instance where you can move the robot around using [interactive markers](http://wiki.ros.org/rviz/Tutorials/Interactive%20Markers%3A%20Getting%20Started) and create plans between poses. You can reference the bottom gif as a guide to plan and execute motion. ![image](images/moveit.gif) -Additionally, the demo allows a user to select from the three groups, *stretch_arm*, *stretch_gripper*, *stretch_head* to move. The option to change groups in the in *Planning Request* section in the *Displays* tree. A few notes to be kept in mind: +Additionally, the demo allows a user to select from the five groups, *stretch_arm*, *stretch_gripper*, *stretch_head*, *mobile_base_arm* and *position* to move. The option to change groups in the in *Planning Request* section in the *Displays* tree. A few notes to be kept in mind: * Pre-defined start and goal states can be specified in Start State and Goal State drop downs in Planning tab of Motion Planning RViz plugin. @@ -39,7 +43,7 @@ Additionally, the demo allows a user to select from the three groups, *stretch_a ## Running Gazebo with MoveIt! and Stretch -```bash + **Next Tutorial:** [Follow Joint Trajectory Commands](follow_joint_trajectory.md) diff --git a/navigation_stack.md b/navigation_stack.md index c27376f..18c4dca 100644 --- a/navigation_stack.md +++ b/navigation_stack.md @@ -1,5 +1,10 @@ ## Navigation Stack with Actual robot +### NOTE +Nav 2 support for Stretch in ROS 2 is under active development. Please reach out to us if you want to use Nav 2 with Stretch in ROS 2. + +Refer to the instructions below if you want to test this functionality in ROS 1. + stretch_navigation provides the standard ROS navigation stack as two launch files. This package utilizes gmapping, move_base, and AMCL to drive the stretch RE1 around a mapped space. Running this code will require the robot to be untethered. diff --git a/rviz_basics.md b/rviz_basics.md index 5c4e259..63af975 100644 --- a/rviz_basics.md +++ b/rviz_basics.md @@ -3,22 +3,24 @@ You can utilize RViz to visualize Stretch's sensor information. To begin, run the stretch driver launch file. ```bash -roslaunch stretch_core stretch_driver.roslaunch +ros2 launch stretch_core stretch_driver.launch.py ``` + Then run the following command to bring up a simple RViz configuration of the Stretch robot. ```bash -rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz +ros2 run rviz2 rviz2 -d `ros2 pkg prefix stretch_calibration`/rviz/stretch_simple_test.rviz ``` An RViz window should open, allowing you to see the various DisplayTypes in the display tree on the left side of the window. ![image](images/simple_rviz.png) -If you want the visualize Stretch's [tf transform tree](http://wiki.ros.org/rviz/DisplayTypes/TF), you need to add the display type to the RViz window. First, click on the *Add* button and include the *TF* type to the display. You will then see all of the transform frames of the Stretch robot and the visualization can be toggled off and on by clicking the checkbox next to the tree. Below is a gif for reference. +If you want to visualize Stretch's [tf transform tree](http://wiki.ros.org/rviz/DisplayTypes/TF), you need to add the display type to the RViz window. First, click on the *Add* button and include the *TF* type to the display. You will then see all of the transform frames of the Stretch robot and the visualization can be toggled off and on by clicking the checkbox next to the tree. Below is a gif for reference. ![image](images/rviz_adding_tf.gif) -There are further tutorials for RViz and can be found [here](http://wiki.ros.org/rviz/Tutorials). +TODO: Add the correct link for working with rviz2 in ROS 2 +There are further tutorials for RViz that can be found [here](http://wiki.ros.org/rviz/Tutorials). ## Running RViz and Gazebo diff --git a/teleoperating_stretch.md b/teleoperating_stretch.md index d736bfb..8fefcd9 100644 --- a/teleoperating_stretch.md +++ b/teleoperating_stretch.md @@ -1,11 +1,15 @@ ## Teleoperating Stretch +### NOTE +Teleoperation support for Stretch in ROS 2 is under active development. Please reach out to us if you want to teleoperate Stretch in ROS 2. + +Refer to the instructions below if you want to test this functionality in ROS 1. + ### Xbox Controller Teleoperating ![image](images/xbox_controller_commands.png) Stretch comes ready to run out of the box. The Xbox Teleoperation demo will let you quickly test out the robot capabilities by teleoperating it with an Xbox Controller. - Note: Make sure the USB Dongle is plugged into the the USB port of the base trunk. To start the demo: