diff --git a/README.md b/README.md index c5005ac..63f8136 100644 --- a/README.md +++ b/README.md @@ -12,10 +12,11 @@ This repo provides instructions on installing and using code on the Stretch RE1 7. [MoveIt! Basics](moveit_basics.md) 8. [Follow Joint Trajectory Commands](follow_joint_trajectory.md) 9. [Perception](perception.md) -10. [FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap) -11. Microphone Array -11. ROS testing -12. Other Nav Stack Features +10. [ArUco Marker Detection](aruco_marker_detection.md) +11. [ReSpeaker Microphone Array](respeaker_microphone_array.md) +12. [FUNMAP](https://github.com/hello-robot/stretch_ros/tree/master/stretch_funmap) +13. ROS testing +14. Other Nav Stack Features ## Other ROS Examples @@ -27,3 +28,9 @@ To help get you get started on your software development, here are examples of n 4. [Give Stretch a Balloon](example_4.md) - Create a "balloon" marker that goes where ever Stretch goes. 5. [Print Joint States](example_5.md) - Print the joint states of Stretch. 6. [Store Effort Values](example_6.md) - Print, store, and plot the effort values of the Stretch robot. +7. [Capture Image](example_7.md) - Capture images from the RealSense camera data. +8. [Voice to Text](example_8.md) - Interpret speech and save transcript to a text file. +9. [Voice Teleoperation of Base](example_9.md) - Use speech to teleoperate the mobile base. +10. [Tf2 Broadcaster and Listener](example_10.md) - Create a tf2 broadcaster and listener. +11. [PointCloud Transformation](example_11.md) - Convert PointCloud2 data to a PointCloud and transform to a different frame. +12. [ArUco Tag Locator](example_12.md) - Actuate the head to locate a requested ArUco marker tag and return a transform. diff --git a/images/hello_robot_favicon.png b/docs/images/hello_robot_favicon.png similarity index 100% rename from images/hello_robot_favicon.png rename to docs/images/hello_robot_favicon.png diff --git a/images/hello_robot_logo_light.png b/docs/images/hello_robot_logo_light.png similarity index 100% rename from images/hello_robot_logo_light.png rename to docs/images/hello_robot_logo_light.png diff --git a/example_1.md b/example_1.md deleted file mode 100644 index fdc6486..0000000 --- a/example_1.md +++ /dev/null @@ -1,160 +0,0 @@ -## 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 the following command in a new terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -Switch the mode to *navigation* mode using a rosservice call. Then drive the robot forward with the move node. - -```bash -# Terminal 2 -rosservice call /switch_to_navigation_mode -cd catkin_ws/src/stretch_ros_tutorials/src/ -python move.py -``` -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. - -```python -#!/usr/bin/env python - -import rospy -from geometry_msgs.msg import Twist - -class Move: - """ - A class that sends Twist messages to move the Stretch robot foward. - """ - def __init__(self): - """ - Function that initializes the subscriber. - :param self: The self reference. - """ - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo - - def move_forward(self): - """ - Function that publishes Twist messages - :param self: The self reference. - - :publishes command: Twist message. - """ - command = Twist() - command.linear.x = 0.1 - 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) - -if __name__ == '__main__': - rospy.init_node('move') - base_motion = Move() - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - base_motion.move_forward() - rate.sleep() -``` - -### The Code Explained - -Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -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. - - -```python -class Move: - def __init__(self): - self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo -``` -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. - - -```Python -command = Twist() -``` -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 -``` -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. - - -```python -command.angular.x = 0.0 -command.angular.y = 0.0 -command.angular.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. - - -```python -self.pub.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() -``` -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. - - -## Move Stretch in Simulation -

- -

- -Using your preferred text editor, modify the topic name of the published *Twist* messages. Please review the edit in the **move.py** script below. - -```python -self.pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=1) -``` - -After saving the edited node, bringup [Stretch in the empty world simulation](gazebo_basics.md). To drive the robot with the node, type the follwing in a new terminal - -```bash -cd catkin_ws/src/stretch_ros_tutorials/src/ -python move.py -``` -To stop the node from sending twist messages, type **Ctrl** + **c**. - - -**Next Example:** [Example 2](example_2.md) diff --git a/example_2.md b/example_2.md deleted file mode 100644 index 14489cd..0000000 --- a/example_2.md +++ /dev/null @@ -1,186 +0,0 @@ -## Example 2 - -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: - -``` -# Laser scans angles are measured counter clockwise, with Stretch's LiDAR having -# both angle_min and angle_max facing forward (very closely along the x-axis) - -Header header -float32 angle_min # start angle of the scan [rad] -float32 angle_max # end angle of the scan [rad] -float32 angle_increment # angular distance between measurements [rad] -float32 time_increment # time between measurements [seconds] -float32 scan_time # time between scans [seconds] -float32 range_min # minimum range value [m] -float32 range_max # maximum range value [m] -float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded) -float32[] intensities # intensity data [device-specific units] -``` -The above message tells you everything you need to know about a scan. Most importantly, you have the angle of each hit and its distance (range) from the scanner. If you want to work with raw range data, then the above message is all you need. There is also an image below that illustrates the components of the message type. - -

- -

- -For a Stretch robot the start angle of the scan, `angle_min`, and -end angle, `angle_max`, are closely located along the x-axis of Stretch's frame. `angle_min` and `angle_max` are set at **-3.1416** and **3.1416**, respectively. This is illustrated by the images below. - -

- - - -

- - -Knowing the orientation of the LiDAR allows us to filter the scan values for a desired range. In this case, we are only considering the scan ranges in front of the stretch robot. - -First, open a terminal and run the stretch driver launch file. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -Then in a new terminal run the rplidar launch file from `stretch_core`. -```bash -# Terminal 2 -roslaunch stretch_core rplidar.launch -``` - -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 -# Terminal 3 -cd catkin_ws/src/stretch_ros_tutorials/src/ -python scan_filter.py -``` - -Then run the following command to bring up a simple RViz configuration of the Stretch robot. -```bash -# Terminal 4 -rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz -``` -Change the topic name from the LaserScan display from */scan* to */filter_scan*. - -

- -

- -### The Code - -```python -#!/usr/bin/env python - -import rospy -from numpy import linspace, inf -from math import sin -from sensor_msgs.msg import LaserScan - -class ScanFilter: - """ - A class that implements a LaserScan filter that removes all of the points - that are not infront of the robot. - """ - def __init__(self): - self.width = 1.0 - self.extent = self.width / 2.0 - self.sub = rospy.Subscriber('/scan', LaserScan, self.callback) - self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10) - rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.") - - def callback(self,msg): - """ - Callback function to deal with incoming laserscan messages. - :param self: The self reference. - :param msg: The subscribed laserscan message. - - :publishes msg: updated laserscan message. - """ - 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) - -if __name__ == '__main__': - rospy.init_node('scan_filter') - ScanFilter() - rospy.spin() -``` - -### The Code Explained - -Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -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. - -```python -self.width = 1 -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) -``` -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) -``` -`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. - -```python -angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) -``` -This line of code utilizes linspace to compute each angle of the subscribed scan. - -```python -points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] -``` -Here we are computing the y coordinates of the ranges that are **below -2.5** and **above 2.5** radians of the scan angles. These limits are sufficient for considering scan ranges in front of Stretch, but these values can be altered to your preference. - -```python -new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] -``` -If the absolute value of a point's y-coordinate is under *self.extent* then keep the range, otherwise use inf, which means "no return". - - -```python -msg.ranges = new_ranges -self.pub.publish(msg) -``` -Substitute in the new ranges in the original message, and republish it. - -```python -rospy.init_node('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 "/". - -Instantiate the class with `ScanFilter()` - -```python -rospy.spin() -``` -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. - -**Previous Example:** [Example 1](example_1.md) -**Next Example:** [Example 3](example_3.md) diff --git a/example_3.md b/example_3.md deleted file mode 100644 index ee72ad7..0000000 --- a/example_3.md +++ /dev/null @@ -1,177 +0,0 @@ -## Example 3 - -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 the following commands in a new terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` -Then in a new terminal type the following to activate the LiDAR sensor. -```bash -# Terminal 2 -roslaunch stretch_core rplidar.launch -``` - -To set *navigation* mode and to activate the avoider node, type the following in a new terminal. - -```bash -# Terminal 3 -rosservice call /switch_to_navigation_mode -cd catkin_ws/src/stretch_ros_tutorials/src/ -python avoider.py -``` -To stop the node from sending twist messages, type **Ctrl** + **c** in the terminal running the avoider node. - -

- -

- -### The Code - -```python -#!/usr/bin/env python - -import rospy -from numpy import linspace, inf, tanh -from math import sin -from geometry_msgs.msg import Twist -from sensor_msgs.msg import LaserScan - -class Avoider: - """ - A class that implements both a LaserScan filter and base velocity control - for collision avoidance. - """ - def __init__(self): - """ - Function that initializes the subscriber, publisher, and marker features. - :param self: The self reference. - """ - 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) - - self.width = 1 - self.extent = self.width / 2.0 - self.distance = 0.5 - - self.twist = Twist() - self.twist.linear.x = 0.0 - self.twist.linear.y = 0.0 - self.twist.linear.z = 0.0 - self.twist.angular.x = 0.0 - self.twist.angular.y = 0.0 - self.twist.angular.z = 0.0 - - def set_speed(self,msg): - """ - Callback function to deal with incoming laserscan messages. - :param self: The self reference. - :param msg: The subscribed laserscan message. - - :publishes self.twist: Twist message. - """ - 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) - -if __name__ == '__main__': - rospy.init_node('avoider') - Avoider() - rospy.spin() -``` - -### The Code Explained - -Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -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. - - -```python -self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo -``` -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. - - -```python -self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed) -``` -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 -self.width = 1 -self.extent = self.width / 2.0 -self.distance = 0.5 -``` -*self.width* is the width of the laser scan we want in front of Stretch. Since Stretch's front is pointing in the x-axis, any points with y coordinates further than half of the defined width (*self.extent*) from the x-axis are not considered. *self.distance* defines the stopping distance from an object. - - -```python -self.twist = Twist() -self.twist.linear.x = 0.0 -self.twist.linear.y = 0.0 -self.twist.linear.z = 0.0 -self.twist.angular.x = 0.0 -self.twist.angular.y = 0.0 -self.twist.angular.z = 0.0 -``` -Allocate a Twist to use, and set everything to zero. We're going to do this when the class is initiating. Redefining this within the callback function, `set_speed()` can be more computationally taxing. - -```python -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)] -``` -This line of code utilizes linspace to compute each angle of the subscribed scan. Here we compute the y coordinates of the ranges that are below -2.5 and above 2.5 radians of the scan angles. These limits are sufficient for considering scan ranges in front of Stretch, but these values can be altered to your preference. If the absolute value of a point's y-coordinate is under self.extent then keep the range, otherwise use inf, which means "no return". - - -```python -error = min(new_ranges) - self.distance -``` -Calculate the difference of the closest measured scan and where we want the robot to stop. We define this as *error*. - - -```python -self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0 -``` -Set the speed according to a tanh function. This method gives a nice smooth mapping from distance to speed, and asymptotes at +/- 1 - - -```python -self.pub.publish(self.twist) -``` -Publish the Twist message. - -```python -rospy.init_node('avoider') -Avoider() -rospy.spin() -``` -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 "/". - -Instantiate class with `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. - -**Previous Example:** [Example 2](example_2.md) -**Next Example:** [Example 4](example_4.md) diff --git a/example_4.md b/example_4.md deleted file mode 100644 index c3c49cc..0000000 --- a/example_4.md +++ /dev/null @@ -1,175 +0,0 @@ -## Example 4 - -![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. - -```bash -# Terminal 1 -roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true -``` -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. - -```bash -# Terminal 2 -cd catkin_ws/src/stretch_ros_tutorials/src/ -python marker.py -``` -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. - -![image](images/balloon.gif) - - -### The Code -```python -#!/usr/bin/env python - -import rospy -from visualization_msgs.msg import Marker - -class Balloon(): - """ - A class that attaches a Sphere marker directly above the Stretch robot. - """ - def __init__(self): - """ - Function that initializes the markers features. - :param self: The self reference. - """ - self.publisher = rospy.Publisher('balloon', Marker, queue_size=10) - self.marker = Marker() - self.marker.header.frame_id = '/base_link' - self.marker.header.stamp = rospy.Time() - 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 - rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.") - - def publish_marker(self): - """ - Function that publishes the sphere marker. - :param self: The self reference. - - :publishes self.marker: Marker message. - """ - self.publisher.publish(self.marker) - - -if __name__ == '__main__': - rospy.init_node('marker', argv=sys.argv) - balloon = Balloon() - rate = rospy.Rate(10) - - while not rospy.is_shutdown(): - balloon.publish_marker() - rate.sleep() -``` - - -### The Code Explained -Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -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. - -```python -self.pub = rospy.Publisher('balloon', Marker, queue_size=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*. - - -```python -self.marker = Marker() -self.marker.header.frame_id = '/base_link' -self.marker.header.stamp = rospy.Time() -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 -self.marker.id = 0 -``` -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 of an existing marker, it will replace the existing marker with that ID number. - -```python -self.marker.action = self.marker.ADD -``` -This line of code sets the action. You can add, delete, or modify markers. - -```python -self.marker.scale.x = 0.5 -self.marker.scale.y = 0.5 -self.marker.scale.z = 0.5 -``` -These are the size parameters for the marker. These will vary by marker type. - -```python -self.marker.color.r = 1.0 -self.marker.color.g = 0.0 -self.marker.color.b = 0.0 -``` -Color of the object, specified as r/g/b/a, with values in the range of [0, 1]. - -```python -self.marker.color.a = 1.0 -``` -The alpha value is from 0 (invisible) to 1 (opaque). If you don't set this then it will automatically default to zero, making your marker invisible. - -```python -self.marker.pose.position.x = 0.0 -self.marker.pose.position.y = 0.0 -self.marker.pose.position.z = 2.0 -``` - -Specify the pose of the marker. Since spheres are rotationally invariant, 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 2 meters above the frame_id (*base_link*), and will move with it. - - -```python -def publish_marker(self): - self.publisher.publish(self.marker) -``` -Publish the Marker data structure to be visualized in RViz. - -```python -rospy.init_node('marker', argv=sys.argv) -balloon = Balloon() -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 "/". - -Instantiate class with `Balloon()` - -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. - - -```python -while not rospy.is_shutdown(): - balloon.publish_marker() - rate.sleep() -``` - -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. - -**Previous Example:** [Example 3](example_3.md) -**Next Example:** [Example 5](example_5.md) diff --git a/example_5.md b/example_5.md deleted file mode 100644 index 7dea114..0000000 --- a/example_5.md +++ /dev/null @@ -1,157 +0,0 @@ -## Example 5 - -In this example, we will review a Python script that prints out the positions of a selected group of Stretch's joints. This script is helpful if you need the joint positions after you teleoperated Stretch with the Xbox controller or physically moved the robot to the desired configuration after hitting the run stop button. - -If you are looking for a continuous print of the joint states while Stretch is in action, then you can use the [rostopic command-line tool](http://wiki.ros.org/rostopic) shown in the [Internal State of Stretch Tutorial](internal_state_of_stretch.md). - -Begin by starting up the stretch driver launch file. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -You can then hit the run-stop button (you should hear a beep and the LED light in the button blink) and move the robot's joints to a desired configuration. Once you are satisfied with the configuration, hold the run-stop button until you hear a beep. Then run the following command to print the joint positions of the lift, arm, and wrist. - -```bash -cd catkin_ws/src/stretch_ros_tutorials/src/ -python joint_state_printer.py -``` -Your terminal will output the `position` information of the previously mentioned joints shown below. -``` -name: ['joint_lift', 'joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3', 'joint_wrist_yaw'] -position: [1.004518435897309, 0.12361581673760723, 0.023224914142933994, 0.07758496706423101, 0.12309362763409384, 1.8771004095879587] - -``` - -It's important to note that the arm has 4 prismatic joints and the sum of these positions gives the wrist extension distance. The wrist extension is needed when sending [joint trajectory commands](follow_joint_trajectory.md) to the robot. Here is an image of the arm joints for reference: - -![image](images/joints.png) - - -### The Code -```python -#!/usr/bin/env python - -import rospy -import sys -from sensor_msgs.msg import JointState - -class JointStatePublisher(): - """ - A class that prints the positions of desired joints in Stretch. - """ - def __init__(self): - """ - Function that initializes the subsriber. - :param self: The self reference. - """ - self.sub = rospy.Subscriber('joint_states', JointState, self.callback) - - def callback(self, msg): - """ - Callback function to deal with the incoming JointState messages. - :param self: The self reference. - :param msg: The JointState message. - """ - self.joint_states = msg - - def print_states(self, joints): - """ - print_states function to deal with the incoming JointState messages. - :param self: The self reference. - :param joints: A list of joint names. - """ - joint_positions = [] - for joint in joints: - index = self.joint_states.name.index(joint) - joint_positions.append(self.joint_states.position[index]) - print("name: " + str(joints)) - print("position: " + str(joint_positions)) - rospy.signal_shutdown("done") - sys.exit(0) - -if __name__ == '__main__': - rospy.init_node('joint_state_printer', anonymous=True) - JSP = JointStatePublisher() - rospy.sleep(.1) - joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_13", "joint_wrist_yaw"] - #joints = ["joint_head_pan","joint_head_tilt", joint_gripper_finger_left", "joint_gripper_finger_right"] - JSP.print_states(joints) - rospy.spin() -``` - - -### The Code Explained -Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -import sys -from sensor_msgs.msg import JointState -``` -You need to import rospy if you are writing a ROS Node. Import sensor_msgs.msg so that we can subscribe to JointState messages. - -```python -self.sub = rospy.Subscriber('joint_states', JointState, self.callback) -``` -Set up a subscriber. We're going to subscribe to the topic "joint_states", looking for JointState messages. When a message comes in, ROS is going to pass it to the function "callback" automatically - -```python -def callback(self, msg): - self.joint_states = msg -``` -This is the callback function where he JointState messages are stored as *self.joint_states*. Further information about the this message type can be found here: [JointState Message](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/JointState.html) - -```python -def print_states(self, joints): - joint_positions = [] - -``` -This is the *print_states()* function which takes in a list of joints of interest as its argument. the is also an empty list set as *joint_positions* and this is where the positions of the requested joints will be appended. - -```python -for joint in joints: - index = self.joint_states.name.index(joint) - joint_positions.append(self.joint_states.position[index]) -print(joint_positions) -``` -In this section of the code, a forloop is used to parse the names of the requested joints from the *self.joint_states* list. The index() function returns the index a of the name of the requested joint and appends the respective position to our *joint_positions* list. - -```python -rospy.signal_shutdown("done") -sys.exit(0) -``` -The first line of code initias a clean shutodwn of ROS. The second line of code exits the Python interpreter. - -```python -rospy.init_node('joint_state_printer', anonymous=True) -JSP = JointStatePublisher() -rospy.sleep(.1) -``` -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 "/". - -Declare object, *JSP*, from the `JointStatePublisher` class. - -The use of the `rospy.sleep()` function is to allow the *JSP* class to initialize all of its features before requesting to publish joint positions of desired joints (running the `print_states()` function). - -```python -joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_13", "joint_wrist_yaw"] -#joints = ["joint_head_pan","joint_head_tilt", joint_gripper_finger_left", "joint_gripper_finger_right"] -JSP.print_states(joints) -``` -Create a list of the desired joints that you want positions to be printed. Then use that list as an argument for the `print_states()` function. - -```python -rospy.spin() -``` -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. - -**Previous Example** [Example 4](example_4.md) -**Next Example** [Example 6](example_6.md) diff --git a/example_6.md b/example_6.md deleted file mode 100644 index d594c9b..0000000 --- a/example_6.md +++ /dev/null @@ -1,307 +0,0 @@ -## Example 6 - -In this example, we will review a Python script that prints out and stores the effort values from a specified joint. If you are looking for a continuous print of the joint state efforts while Stretch is in action, then you can use the [rostopic command-line tool](http://wiki.ros.org/rostopic) shown in the [Internal State of Stretch Tutorial](internal_state_of_stretch.md). - - -![image](images/effort_sensing.gif) - - -Begin by running the following command in the terminal in a terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -Switch the mode to *manipulation* mode using a rosservice call. Then run the single effort sensing node. - -```bash -# Terminal 2 -rosservice call /switch_to_manipulation_mode -cd catkin_ws/src/stretch_ros_tutorials/src/ -python effort_sensing.py -``` - -This will send a `FollowJointTrajectory` command to move Stretch's arm or head while also printing the effort of the lift. - -### The Code -```python -#!/usr/bin/env python -import rospy -import time -import actionlib -import os -import csv -from datetime import datetime - -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -from sensor_msgs.msg import JointState -import hello_helpers.hello_misc as hm - -class JointActuatorEffortSensor(hm.HelloNode): - """ - A class that sends multiple joint trajectory goals to a single joint. - """ - def __init__(self): - """ - Function that initializes the subscriber,and other features. - :param self: The self reference. - """ - hm.HelloNode.__init__(self) - self.sub = rospy.Subscriber('joint_states', JointState, self.callback) - self.joints = ['joint_lift'] - self.joint_effort = [] - self.save_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data' - self.export_data = False - - def callback(self, msg): - """ - Callback function to update and store JointState messages. - :param self: The self reference. - :param msg: The JointState message. - """ - self.joint_states = msg - - def issue_command(self): - """ - Function that makes an action call and sends joint trajectory goals - to a single joint. - :param self: The self reference. - """ - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = self.joints - - point0 = JointTrajectoryPoint() - point0.positions = [0.9] - - trajectory_goal.trajectory.points = [point0] - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' - self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - def feedback_callback(self,feedback): - """ - The feedback_callback function deals with the incoming feedback messages - from the trajectory_client. Although, in this function, we do not use the - feedback information. - :param self: The self reference. - :param feedback: FollowJointTrajectoryActionFeedback message. - """ - if 'wrist_extension' in self.joints: - self.joints.remove('wrist_extension') - self.joints.append('joint_arm_l0') - - current_effort = [] - for joint in self.joints: - index = self.joint_states.name.index(joint) - current_effort.append(self.joint_states.effort[index]) - - if not self.export_data: - print("name: " + str(self.joints)) - print("effort: " + str(current_effort)) - else: - self.joint_effort.append(current_effort) - - - def done_callback(self, status, result): - """ - The done_callback function will be called when the joint action is complete. - Within this function we export the data to a .txt file in the /stored_data directory. - :param self: The self reference. - :param status: status attribute from FollowJointTrajectoryActionResult message. - :param result: result attribute from FollowJointTrajectoryActionResult message. - """ - if status == actionlib.GoalStatus.SUCCEEDED: - rospy.loginfo('Suceeded') - else: - rospy.loginfo('Failed') - - if self.export_data: - file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p") - completeName = os.path.join(self.save_path, file_name) - with open(completeName, "w") as f: - writer = csv.writer(f) - writer.writerow(self.joints) - writer.writerows(self.joint_effort) - - def main(self): - """ - Function that initiates the issue_command function. - :param self: The self reference. - """ - hm.HelloNode.main(self, 'issue_command', 'issue_command', wait_for_first_pointcloud=False) - rospy.loginfo('issuing command...') - self.issue_command() - time.sleep(2) - -if __name__ == '__main__': - try: - node = JointActuatorEffortSensor() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') - -``` - - -### The Code Explained -This code is similar to that of the [multipoint_command](https://github.com/hello-sanchez/stretch_ros_tutorials/blob/main/src/multipoint_command.py) and [joint_state_printer](https://github.com/hello-sanchez/stretch_ros_tutorials/blob/main/src/joint_state_printer.py) node. Therefore, this example will highlight sections that are different from that tutorial. Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -import time -import actionlib -import os -import csv -from datetime import datetime - -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -from sensor_msgs.msg import JointState -import hello_helpers.hello_misc as hm -``` - -You need to import rospy if you are writing a ROS Node. Import the `FollowJointTrajectoryGoal` from the `control_msgs.msg` package to control the Stretch robot. Import `JointTrajectoryPoint` from the `trajectory_msgs` package to define robot trajectories. The `hello_helpers` package consists of a module that provides various Python scripts used across stretch_ros. In this instance, we are importing the `hello_misc` script. - - -```Python -class JointActuatorEffortSensor(hm.HelloNode): - """ - A class that sends multiple joint trajectory goals to a single joint. - """ - def __init__(self): - """ - Function that initializes the subscriber,and other features. - :param self: The self reference. - """ - hm.HelloNode.__init__(self) -``` -The `JointActuatorEffortSensor ` class inherits the `HelloNode` class from `hm` and is initialized. - -```python -self.sub = rospy.Subscriber('joint_states', JointState, self.callback) -self.joints = ['joint_lift'] - -``` -Set up a subscriber. We're going to subscribe to the topic "joint_states", looking for `JointState` messages. When a message comes in, ROS is going to pass it to the function "callback" automatically. Create a list of the desired joints you want to print. -```Python -self.joint_effort = [] -self.save_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data' -self.export_data = False -``` - -Create an empty list to store the joint effort values. The *self.save_path* is the directory path where the .txt file of the effort values will be stored. You can change this path to a preferred directory. The *self.export_data* is a boolean and its default value is set to False. If set to True, then the joint values will be stored in a .txt file, otherwise, the values will be printed in the terminal where you ran the effort sensing node. -```python -self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback) -``` - -Include the feedback and done call back functions in the send goal function. - -```python -def feedback_callback(self,feedback): - """ - The feedback_callback function deals with the incoming feedback messages - from the trajectory_client. Although, in this function, we do not use the - feedback information. - :param self: The self reference. - :param feedback: FollowJointTrajectoryActionFeedback message. - """ -``` - -The feedback callback function takes in the `FollowJointTrajectoryActionFeedback` message as its argument. - -```python -if 'wrist_extension' in self.joints: - self.joints.remove('wrist_extension') - self.joints.append('joint_arm_l0') -``` - -Use a conditional statement to replace `wrist_extenstion` to `joint_arm_l0`. This is because `joint_arm_l0` has the effort values that the `wrist_extension` is experiencing. - -```python -current_effort = [] -for joint in self.joints: - index = self.joint_states.name.index(joint) - current_effort.append(self.joint_states.effort[index]) -``` - -Create an empty list to store the current effort values. Then use a for loop to parse the joint names and effort values. - -```python -if not self.export_data: - print("name: " + str(self.joints)) - print("effort: " + str(current_effort)) -else: - self.joint_effort.append(current_effort) -``` -Use a conditional statement to print effort values in the terminal or store values into a list that will be used for exporting the data in a .txt file. - -```Python -def done_callback(self, status, result): - """ - The done_callback function will be called when the joint action is complete. - Within this function we export the data to a .txt file in the /stored_data directory. - :param self: The self reference. - :param status: status attribute from FollowJointTrajectoryActionResult message. - :param result: result attribute from FollowJointTrajectoryActionResult message. - """ -``` - -The done callback function takes in the FollowJointTrajectoryActionResult messages as its arguments. - -```python -if status == actionlib.GoalStatus.SUCCEEDED: - rospy.loginfo('Suceeded') -else: - rospy.loginfo('Failed') - -``` - -Conditional statement to print whether the goal status in the `FollowJointTrajectoryActionResult` succeeded or failed. - -```python -if self.export_data: - file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p") - completeName = os.path.join(self.save_path, file_name) - - with open(completeName, "w") as f: - writer = csv.writer(f) - writer.writerow(self.joints) - writer.writerows(self.joint_effort) - -``` - -A conditional statement is used to export the data to a .txt file. The file's name is set to the date and time the node was executed. That way, no previous files are overwritten. - - -### Plotting/Animating Effort Data - -![image](stored_data/2022-06-30_11:26:20-AM.png) - -We added a simple python script, [stored_data_plotter.py](https://github.com/hello-sanchez/stretch_ros_tutorials/blob/main/src/stored_data_plotter.py), to this package for plotting the stored data. Note you have to change the name of the file you wish to see in the python script. This is shown below: - -```Python -####################### Copy the file name here! ####################### -file_name = '2022-06-30_11:26:20-AM' -``` - -Once you have changed the file name, then run the following in a new command. - -```bash -cd catkin_ws/src/stretch_ros_tutorials/src/ -python stored_data_plotter.py - -``` - -Because this is not a node, you don't need roscore to run this script. Please review the comments in the python script for additional guidance. - -**Previous Example** [Example 5](example_4.md) diff --git a/docs/extra.css b/extra.css similarity index 100% rename from docs/extra.css rename to extra.css diff --git a/follow_joint_trajectory.md b/follow_joint_trajectory.md deleted file mode 100644 index 59192db..0000000 --- a/follow_joint_trajectory.md +++ /dev/null @@ -1,415 +0,0 @@ -## 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. - -## Stow Command Example -

- -

- - - -Begin by running the following command in the terminal in a terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -Switch the mode to *manipulation* mode using a rosservice call. Then run the stow command node. - -```bash -# Terminal 2 -rosservice call /switch_to_manipulation_mode -cd catkin_ws/src/stretch_ros_tutorials/src/ -python stow_command.py -``` - - -This will send a `FollowJointTrajectory` command to stow Stretch's arm. -### The Code - -```python -#!/usr/bin/env python - -import rospy -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm -import time - -class StowCommand(hm.HelloNode): - ''' - A class that sends a joint trajectory goal to stow the Stretch's arm. - ''' - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_stow_command(self): - ''' - Function that makes an action call and sends stow postion goal. - :param self: The self reference. - ''' - stow_point = JointTrajectoryPoint() - stow_point.time_from_start = rospy.Duration(0.000) - stow_point.positions = [0.2, 0.0, 3.4] - - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - trajectory_goal.trajectory.points = [stow_point] - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' - - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - def main(self): - ''' - Function that initiates stow_command function. - :param self: The self reference. - ''' - hm.HelloNode.main(self, 'stow_command', 'stow_command', wait_for_first_pointcloud=False) - rospy.loginfo('stowing...') - self.issue_stow_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - node = StowCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') -``` - -### The Code Explained - -Now let's break the code down. - -```python -#!/usr/bin/env python -``` -Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at the top. The first line makes sure your script is executed as a Python script. - - -```python -import rospy -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm -import time -``` -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. - -```python -class StowCommand(hm.HelloNode): - - def __init__(self): - hm.HelloNode.__init__(self) -``` -The `StowCommand ` class inherits the `HelloNode` class from `hm` 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. - -```python - 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' -``` -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. - -```python -self.trajectory_client.send_goal(trajectory_goal) -rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) -self.trajectory_client.wait_for_result() -``` -Make the action call and send the goal. The last line of code waits for the result before it exits the python script. - -```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) -``` -Create a funcion, `main()`, to do all of the setup the `hm.HelloNode` class and issue the stow command. - -```python -if __name__ == '__main__': - try: - node = StowCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') - -``` -Declare object, *node*, from the `StowCommand()` class. Then run the `main()` function. - - -## Multipoint Command Example - -

- -

- -Begin by running the following command in the terminal in a terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -Switch the mode to *manipulation* mode using a rosservice call. Then run the multipoint command node. - -```bash -# Terminal 2 -rosservice call /switch_to_manipulation_mode -cd catkin_ws/src/stretch_ros_tutorials/src/ -python multipoint_command.py -``` - -This will send a list of `JointTrajectoryPoint` message types to move Stretch's arm. - -### The Code -```python -#!/usr/bin/env python - -import rospy -import time -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm - -class MultiPointCommand(hm.HelloNode): - """ - A class that sends multiple joint trajectory goals to the stretch robot. - """ - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_multipoint_command(self): - """ - Function that makes an action call and sends multiple joint trajectory goals - to the joint_lift, wrist_extension, and joint_wrist_yaw. - :param self: The self reference. - """ - point0 = JointTrajectoryPoint() - point0.positions = [0.2, 0.0, 3.4] - point0.velocities = [0.2, 0.2, 2.5] - point0.accelerations = [1.0, 1.0, 3.5] - - point1 = JointTrajectoryPoint() - point1.positions = [0.3, 0.1, 2.0] - - 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] - - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw'] - trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5] - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' - - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent list of goals = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - def main(self): - """ - Function that initiates the multipoint_command function. - :param self: The self reference. - """ - hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False) - rospy.loginfo('issuing multipoint command...') - self.issue_multipoint_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - node = MultiPointCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') - -``` - -### The Code Explained -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] -``` -Set *point0* as a `JointTrajectoryPoint`and provide desired positions. These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. The lift and wrist extension positions are expressed in meters, where as the wrist yaw is in radians. - - -```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 accelerations of the lift (m/s^2), wrist extension (m/s^2), and wrist yaw (rad/s^2). - -**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 -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' -``` -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. - -## Single Joint Actuator - - -

- -

- -You can also actuate a single joint for the Stretch. Below are the list of joints and their position limit that can be moved in the *position* mode. - -``` -############################# Joint limits ############################# -joint_lift: lower_limit = 0.15, upper_limit = 1.10 # in meters -wrist_extension: lower_limit = 0.00, upper_limit = 0.50 # in meters -joint_wrist_yaw: lower_limit = -1.75, upper_limit = 4.00 # in radians -joint_head_pan: lower_limit = -2.80, upper_limit = 2.90 # in radians -joint_head_tilt: lower_limit = -1.60, upper_limit = 0.40 # in radians -joint_gripper_finger_left: lower_limit = -0.35, upper_limit = 0.165 # in radians -######################################################################## -``` - -Begin by running the following command in the terminal in a terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -Switch the mode to *manipulation* mode using a rosservice call. Then run the single joint actuator node. - -```bash -# Terminal 2 -rosservice call /switch_to_manipulation_mode -cd catkin_ws/src/stretch_ros_tutorials/src/ -python single_joint_actuator.py -``` - -This will send a list of `JointTrajectoryPoint` message types to move Stretch's arm. - - -The joint, *joint_gripper_finger_left*, is only needed when actuating the gripper. - - -### The Code -```python -#!/usr/bin/env python - -import rospy -import time -from control_msgs.msg import FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint -import hello_helpers.hello_misc as hm - -class SingleJointActuator(hm.HelloNode): - """ - A class that sends multiple joint trajectory goals to a single joint. - """ - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_command(self): - """ - Function that makes an action call and sends joint trajectory goals - to a single joint - :param self: The self reference. - """ - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_head_pan'] - - point0 = JointTrajectoryPoint() - point0.positions = [0.65] - - # point1 = JointTrajectoryPoint() - # point1.positions = [0.5] - - trajectory_goal.trajectory.points = [point0]#, point1] - trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) - trajectory_goal.trajectory.header.frame_id = 'base_link' - self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Sent goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - def main(self): - """ - Function that initiates the issue_command function. - :param self: The self reference. - """ - hm.HelloNode.main(self, 'issue_command', 'issue_command', wait_for_first_pointcloud=False) - rospy.loginfo('issuing command...') - self.issue_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - node = SingleJointActuator() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') -``` -### The Code Explained - -Since the code is quite similar to the multipoint_command code, we will only review the parts that differ. - -Now let's break the code down. - -```python -trajectory_goal = FollowJointTrajectoryGoal() -trajectory_goal.trajectory.joint_names = ['joint_head_pan'] -``` -Here we only input joint name that we want to actuate. In this instance, we will actuate the *joint_head_pan*. - -```python -point0 = JointTrajectoryPoint() -point0.positions = [0.65] - -# point1 = JointTrajectoryPoint() -# point1.positions = [0.5] -``` -Set *point0* as a `JointTrajectoryPoint`and provide desired position. You also have the option to send multiple point positions rather than one. - -```python -trajectory_goal.trajectory.points = [point0]#, point1] -trajectory_goal.trajectory.header.stamp = rospy.Time(0.0) -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` set by your list of points. Specify the coordinate frame that we want (base_link) and set the time to be now. - - -**Next Tutorial:** [Perception](perception.md) diff --git a/gazebo_basics.md b/gazebo_basics.md deleted file mode 100644 index 6eefe35..0000000 --- a/gazebo_basics.md +++ /dev/null @@ -1,28 +0,0 @@ -# Spawning Stretch in Simulation (Gazebo) - -### Empty World Simulation -To spawn the Stretch in gazebo's default empty world run the following command in your terminal. -```bash -roslaunch stretch_gazebo gazebo.launch -``` -This will bringup the robot in the gazebo simulation similar to the image shown below. - - -![image](images/stretch_gazebo_empty_world.png) - -### Custom World Simulation -In gazebo, you are able to spawn Stretch in various worlds. First, source the gazebo world files by running the following command in a terminal -```bash -echo "source /usr/share/gazebo/setup.sh" -``` - - -Then using the world argument, you can spawn the stretch in the willowgarage world by running the following - -```bash -roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world -``` - -![image](images/stretch_willowgarage_world.png) - -**Next Tutorial:** [Teleoperating Stretch](teleoperating_stretch.md) diff --git a/getting_started.md b/getting_started.md deleted file mode 100644 index 8a7cf07..0000000 --- a/getting_started.md +++ /dev/null @@ -1,57 +0,0 @@ -# Getting Started - -## Ubuntu - -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). Then create a catkin workspace for your ROS packages. Here is an [installation guide for creating a workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). Once your system is set up, clone the [stretch_ros_tutorials](https://github.com/hello-sanchez/stretch_ros_tutorials.git) to your workspace and build the package in your workspace. This can be done by copying the commands below and pasting them into your terminal. - -```bash -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 ROS Noetic. To begin the setup, start with [installing Ubuntu 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. - -Currently, the Realsense2_description package isn't installed by rosdep and requires a user to install the package manually. Run the following command in your terminal - -```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 **src** folder in your preferred workspace. -```bash -cd ~/catkin_ws/src -git clone https://github.com/hello-robot/stretch_ros -git clone https://github.com/pal-robotics/realsense_gazebo_plugin -git clone https://github.com/hello-sanchez/stretch_ros_tutorials.git -``` - - -Change the directory to that of your catkin workspace and install system dependencies of the ROS packages. Then build your workspace. -```bash -cd ~/catkin_ws -rosdep install --from-paths src --ignore-src -r -y -catkin_make -``` - - -Once `caktin_make` has finished compiling,source your workspace and **.bashrc** file -```bash -echo "source /home/USER_NAME/catkin_ws/devel/setup.bash" -source ~/.bashrc -``` - -## RoboMaker - -![image](images/aws-robomaker.png) - - -If you cannot 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 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. - -**Next Tutorial:** [Gazebo Basics](gazebo_basics.md) diff --git a/docs/getting_started/quick_start_guide.md b/getting_started/quick_start_guide.md similarity index 100% rename from docs/getting_started/quick_start_guide.md rename to getting_started/quick_start_guide.md diff --git a/docs/getting_started/robot_safety_guide.md b/getting_started/robot_safety_guide.md similarity index 100% rename from docs/getting_started/robot_safety_guide.md rename to getting_started/robot_safety_guide.md diff --git a/docs/getting_started/troubleshooting_guide.md b/getting_started/troubleshooting_guide.md similarity index 100% rename from docs/getting_started/troubleshooting_guide.md rename to getting_started/troubleshooting_guide.md diff --git a/docs/getting_started/untethered_operation.md b/getting_started/untethered_operation.md similarity index 100% rename from docs/getting_started/untethered_operation.md rename to getting_started/untethered_operation.md diff --git a/docs/getting_started/updating_software.md b/getting_started/updating_software.md similarity index 100% rename from docs/getting_started/updating_software.md rename to getting_started/updating_software.md diff --git a/images/after_launch_file_edit.png b/images/after_launch_file_edit.png deleted file mode 100644 index 673e0b6..0000000 Binary files a/images/after_launch_file_edit.png and /dev/null differ diff --git a/images/avoider.gif b/images/avoider.gif deleted file mode 100644 index 1215760..0000000 Binary files a/images/avoider.gif and /dev/null differ diff --git a/images/aws-robomaker.png b/images/aws-robomaker.png deleted file mode 100644 index 54bf86a..0000000 Binary files a/images/aws-robomaker.png and /dev/null differ diff --git a/images/balloon.gif b/images/balloon.gif deleted file mode 100644 index 665b98c..0000000 Binary files a/images/balloon.gif and /dev/null differ diff --git a/images/balloon.png b/images/balloon.png deleted file mode 100644 index 09b8883..0000000 Binary files a/images/balloon.png and /dev/null differ diff --git a/images/before_launch_file_edit.png b/images/before_launch_file_edit.png deleted file mode 100644 index 76a6b97..0000000 Binary files a/images/before_launch_file_edit.png and /dev/null differ diff --git a/images/effort_sensing.gif b/images/effort_sensing.gif deleted file mode 100644 index 0919397..0000000 Binary files a/images/effort_sensing.gif and /dev/null differ diff --git a/images/gazebo_mapping.gif b/images/gazebo_mapping.gif deleted file mode 100644 index c375888..0000000 Binary files a/images/gazebo_mapping.gif and /dev/null differ diff --git a/images/gazebo_moveit.gif b/images/gazebo_moveit.gif deleted file mode 100644 index 472ba55..0000000 Binary files a/images/gazebo_moveit.gif and /dev/null differ diff --git a/images/joints.png b/images/joints.png deleted file mode 100644 index aaf3d41..0000000 Binary files a/images/joints.png and /dev/null differ diff --git a/images/lidar.png b/images/lidar.png deleted file mode 100644 index 363099b..0000000 Binary files a/images/lidar.png and /dev/null differ diff --git a/images/mapping.gif b/images/mapping.gif deleted file mode 100644 index 5494646..0000000 Binary files a/images/mapping.gif and /dev/null differ diff --git a/images/move.gif b/images/move.gif deleted file mode 100644 index fb74abf..0000000 Binary files a/images/move.gif and /dev/null differ diff --git a/images/move_stretch.gif b/images/move_stretch.gif deleted file mode 100644 index 8c169c8..0000000 Binary files a/images/move_stretch.gif and /dev/null differ diff --git a/images/moveit.gif b/images/moveit.gif deleted file mode 100644 index 7ffdf5e..0000000 Binary files a/images/moveit.gif and /dev/null differ diff --git a/images/moveit_groups.gif b/images/moveit_groups.gif deleted file mode 100644 index 6921aa6..0000000 Binary files a/images/moveit_groups.gif and /dev/null differ diff --git a/images/multipoint.gif b/images/multipoint.gif deleted file mode 100644 index f67738a..0000000 Binary files a/images/multipoint.gif and /dev/null differ diff --git a/images/perception_camera.gif b/images/perception_camera.gif deleted file mode 100644 index 597a319..0000000 Binary files a/images/perception_camera.gif and /dev/null differ diff --git a/images/perception_depth.gif b/images/perception_depth.gif deleted file mode 100644 index 4a05130..0000000 Binary files a/images/perception_depth.gif and /dev/null differ diff --git a/images/perception_image.gif b/images/perception_image.gif deleted file mode 100644 index 9e925ff..0000000 Binary files a/images/perception_image.gif and /dev/null differ diff --git a/images/perception_rviz.gif b/images/perception_rviz.gif deleted file mode 100644 index f529a73..0000000 Binary files a/images/perception_rviz.gif and /dev/null differ diff --git a/images/rosgraph.png b/images/rosgraph.png deleted file mode 100644 index be302ea..0000000 Binary files a/images/rosgraph.png and /dev/null differ diff --git a/images/rqt_graph.png b/images/rqt_graph.png deleted file mode 100644 index 3948795..0000000 Binary files a/images/rqt_graph.png and /dev/null differ diff --git a/images/rviz_adding_tf.gif b/images/rviz_adding_tf.gif deleted file mode 100644 index 567ad22..0000000 Binary files a/images/rviz_adding_tf.gif and /dev/null differ diff --git a/images/scan_angles.png b/images/scan_angles.png deleted file mode 100644 index 9670e33..0000000 Binary files a/images/scan_angles.png and /dev/null differ diff --git a/images/scanfilter.gif b/images/scanfilter.gif deleted file mode 100644 index cfc99fa..0000000 Binary files a/images/scanfilter.gif and /dev/null differ diff --git a/images/simple_rviz.png b/images/simple_rviz.png deleted file mode 100644 index 708e579..0000000 Binary files a/images/simple_rviz.png and /dev/null differ diff --git a/images/single_joint_actuator.gif b/images/single_joint_actuator.gif deleted file mode 100644 index 0576bb2..0000000 Binary files a/images/single_joint_actuator.gif and /dev/null differ diff --git a/images/stow_command.gif b/images/stow_command.gif deleted file mode 100644 index 20a0a14..0000000 Binary files a/images/stow_command.gif and /dev/null differ diff --git a/images/stretch_axes.png b/images/stretch_axes.png deleted file mode 100644 index 98e37c8..0000000 Binary files a/images/stretch_axes.png and /dev/null differ diff --git a/images/stretch_gazebo_empty_world.png b/images/stretch_gazebo_empty_world.png deleted file mode 100644 index 9a5dfc6..0000000 Binary files a/images/stretch_gazebo_empty_world.png and /dev/null differ diff --git a/images/stretch_willowgarage_world.png b/images/stretch_willowgarage_world.png deleted file mode 100644 index a42a1ca..0000000 Binary files a/images/stretch_willowgarage_world.png and /dev/null differ diff --git a/images/willowgarage_with_rviz.png b/images/willowgarage_with_rviz.png deleted file mode 100644 index b4d7576..0000000 Binary files a/images/willowgarage_with_rviz.png and /dev/null differ diff --git a/images/xbox_controller_commands.png b/images/xbox_controller_commands.png deleted file mode 100644 index 08dab81..0000000 Binary files a/images/xbox_controller_commands.png and /dev/null differ diff --git a/internal_state_of_stretch.md b/internal_state_of_stretch.md deleted file mode 100644 index 364d8e2..0000000 --- a/internal_state_of_stretch.md +++ /dev/null @@ -1,61 +0,0 @@ -## Getting the State of the Robot - -Begin by starting up the stretch driver launch file. -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -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. -```bash -# Terminal 2 -rostopic echo /joint_states -n1 -``` -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. Remove the flag if you prefer to continuously print the topic for debugging purposes. - -Your terminal will output the information associated with the `/joint_states` topic. Your `header`, `position`, `velocity`, and `effort` information may vary from what is printed below. -``` -header: - seq: 70999 - stamp: - secs: 1420 - nsecs: 2000000 - frame_id: '' -name: [joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, joint_gripper_finger_left, - joint_gripper_finger_right, joint_head_pan, joint_head_tilt, joint_left_wheel, joint_lift, - joint_right_wheel, joint_wrist_yaw] -position: [-1.6137320244357253e-08, -2.9392484829061376e-07, -2.8036125938539207e-07, -2.056847528567165e-07, -2.0518734302754638e-06, -5.98271107676851e-06, 2.9291786329821434e-07, 1.3802900147297237e-06, 0.08154086954434359, 1.4361499260374905e-07, 0.4139061738340768, 9.32603306580404e-07] -velocity: [0.00015598730463972836, -0.00029395074514369584, -0.0002803845454217379, 1.322424459109634e-05, -0.00035084643762840415, 0.0012164337445918797, 0.0002138814988808099, 0.00010419792027496809, 4.0575263146426684e-05, 0.00022487596895736357, -0.0007751929074042957, 0.0002451588607332439] -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 -# Terminal 2 -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 published active topics. - -A powerful tool to visualize the ROS communication is the ROS [rqt_graph package](http://wiki.ros.org/rqt_graph). By typing the following, you can see a graph of topics being communicated between nodes. - -```bash -# Terminal 3 -rqt_graph -``` -![image](images/rqt_graph.png) - -The graph allows a user to observe and affirm if topics are broadcasted to the correct nodes. This method can also be utilized to debug communication issues. - -**Next Tutorial:** [RViz Basics](rviz_basics.md) diff --git a/moveit_basics.md b/moveit_basics.md deleted file mode 100644 index 6433739..0000000 --- a/moveit_basics.md +++ /dev/null @@ -1,57 +0,0 @@ -# MoveIt! Basics - - -## MoveIt! Without Hardware -To begin running MoveIt! on stretch, run the demo launch file. This doesn't require any simulator or robot to run. - -```bash -roslaunch stretch_moveit_config demo.launch -``` -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: - -* 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. - -* *stretch_gripper* group does not show markers, and is intended to be controlled via the joints tab that is located in the very right of Motion Planning Rviz plugin. - -* When planning with *stretch_head* group make sure you select *Approx IK Solutions* in Planning tab of Motion Planning RViz plugin. - - -![image](images/moveit_groups.gif) - - -## Running Gazebo with MoveIt! and Stretch - - -```bash -# Terminal 1: -roslaunch stretch_gazebo gazebo.launch -# Terminal 2: -roslaunch stretch_core teleop_twist.launch twist_topic:=/stretch_diff_drive_controller/cmd_vel linear:=1.0 angular:=2.0 teleop_type:=keyboard # or use teleop_type:=joystick if you have a controller -# Terminal 3 -roslaunch stretch_moveit_config demo_gazebo.launch -``` - - -This will launch an Rviz instance that visualizes the joints with markers and an empty world in Gazebo with Stretch and load all the controllers. There are pre-defined positions for each joint group for demonstration purposes. There are three joint groups, namely stretch_arm, stretch_gripper and stretch_head that can be controlled individually via Motion Planning Rviz plugin. Start and goal positions for joints can be selected similar to [this moveit tutorial](https://ros-planning.github.io/moveit_tutorials/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html#choosing-specific-start-goal-states). - - -![image](images/gazebo_moveit.gif) - -**Next Tutorial:** [Follow Joint Trajectory Commands](follow_joint_trajectory.md) diff --git a/navigation_stack.md b/navigation_stack.md deleted file mode 100644 index 39ccbbb..0000000 --- a/navigation_stack.md +++ /dev/null @@ -1,74 +0,0 @@ -## Navigation Stack with Actual robot - -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. - - -Then run the following commands to map the space that the robot will navigate in. -```bash -roslaunch stretch_navigation mapping.launch -``` -Rviz will show the robot and the map that is being constructed. With the terminal open, use the instructions printed by the teleop package to teleoperate the robot around the room. Avoid sharp turns and revisit previously visited spots to form loop closures. - -

- -

- -In Rviz, once you see a map that has reconstructed the space well enough, you can run the following commands to save the map to `stretch_user/`. - -```bash -mkdir -p ~/stretch_user/maps -rosrun map_server map_saver -f ${HELLO_FLEET_PATH}/maps/ -``` - -The `` does not include an extension. Map_saver will save two files as `.pgm` and `.yaml`. - -Next, with `.yaml`, we can navigate the robot around the mapped space. Run: - -```bash -roslaunch stretch_navigation navigation.launch map_yaml:=${HELLO_FLEET_PATH}/maps/.yaml -``` - -Rviz will show the robot in the previously mapped space; however, the robot's location on the map does not match the robot's location in the real space. In the top bar of Rviz, use 2D Pose Estimate to lay an arrow down roughly where the robot is located in the real space. AMCL, the localization package, will better localize our pose once we give the robot a 2D Nav Goal. In the top bar of Rviz, use 2D Nav Goal to lay down an arrow where you'd like the robot to go. In the terminal, you'll see move_base go through the planning phases and then navigate the robot to the goal. If planning fails, the robot will begin a recovery behavior: spinning around 360 degrees in place. - -It is also possible to send 2D Pose Estimates and Nav Goals programmatically. In your launch file, you may include `navigation.launch` to bring up the navigation stack. Then, you can send *move_base_msgs::MoveBaseGoal* messages in order to navigate the robot programmatically. - - - -## Running in Simulation - -To perform mapping and navigation in the Gazebo simulation of Stretch, substitute the `mapping_gazebo.launch` and `navigation_gazebo.launch` launch files into the commands above. The default Gazebo environment is the Willow Garage HQ. Use the "world" ROS argument to specify the Gazebo world within which to spawn Stretch. - -```bash -# Terminal 1 -roslaunch stretch_navigation mapping_gazebo.launch gazebo_world:=worlds/willowgarage.world -``` - -### Teleop using a Joystick Controller - -The mapping launch files, `mapping.launch` and `mapping_gazebo.launch` expose the ROS argument, "teleop_type". By default, this ROS arg is set to "keyboard", which launches keyboard teleop in the terminal. If the xbox controller that ships with Stretch RE1 is plugged into your computer, the following command will launch mapping with joystick teleop: - -```bash -# Terminal 2 -roslaunch stretch_navigation mapping.launch teleop_type:=joystick -``` - -

- -

- -### Using ROS Remote Master - -If you have set up [ROS Remote Master](https://docs.hello-robot.com/untethered_operation/#ros-remote-master) for [untethered operation](https://docs.hello-robot.com/untethered_operation/), you can use Rviz and teleop locally with the following commands: - -```bash -# On Robot -roslaunch stretch_navigation mapping.launch rviz:=false teleop_type:=none - -# On your machine, Terminal 1: -rviz -d `rospack find stretch_navigation`/rviz/mapping.launch -# On your machine, Terminal 2: -roslaunch stretch_core teleop_twist.launch teleop_type:=keyboard # or use teleop_type:=joystick if you have a controller -``` - - -**Next Tutorial:** [MoveIt! Basics](moveit_basics.md) diff --git a/perception.md b/perception.md deleted file mode 100644 index 181c138..0000000 --- a/perception.md +++ /dev/null @@ -1,59 +0,0 @@ -# Perception Introduction - -The Stretch robot is equipped with the Intel RealSense D435i camera, an essential component that allows the robot to measure and analyze the world around it. In this tutorial, we are going to showcase how to visualize the various topics published from the camera. - -Begin by checking out the [feature/upright_camera_view](https://github.com/hello-robot/stretch_ros/tree/feature/upright_camera_view) branch in the stretch_ros repository. The configuration of the camera results in the images being displayed sideways. Thus, this branch publishes a new topic that rotates the raw image upright. - -```bash -cd ~/catkin_ws/src/stretch_ros/stretch_core -git checkout feature/upright_camera_view -``` -Then run the stretch driver launch file. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -To activate the RealSense camera and publish topics to be visualized, run the following launch file in a new terminal. - -```bash -# Terminal 2 -roslaunch stretch_core d435i_low_resolution.launch -``` - -Within this tutorial package, there is an RViz config file with the topics for perception already in the Display tree. You can visualize these topics and the robot model by running the command below in a new terminal. - -```bash -# Terminal 3 -rosrun rviz rviz -d /home/hello-robot/catkin_ws/src/stretch_ros_tutorials/rviz/perception_example.rviz -``` - -## PointCloud2 Display - -A list of displays on the left side of the interface can visualize the camera data. Each display has its properties and status that notify a user if topic messages are received. - -For the `PointCloud2` display, a [sensor_msgs/pointCloud2](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/PointCloud2.html) message named */camera/depth/color/points*, is received and the gif below demonstrates the various display properties when visualizing the data. - - -

- -

- -## Image Display -The `Image` display when toggled creates a new rendering window that visualizes a [sensor_msgs/Image](http://docs.ros.org/en/lunar/api/sensor_msgs/html/msg/Image.html) messaged, */camera/color/image/raw*. This feature shows the image data from the camera; however, the image comes out sideways. Thus, you can select the */camera/color/image/raw_upright_view* from the **Image Topic** options to get an upright view of the image. -

- -

- -## Camera Display -The `Camera` display is similar to that of the `Image` display. In this setting, the rendering window also visualizes other displays, such as the PointCloud2, the RobotModel, and Grid Displays. The **visibility** property can toggle what displays your are interested in visualizing. -

- -

- -## DepthCloud Display -The `DepthCloud` display is visualized in the main RViz window. This display takes in the depth image and RGB image, provided by the RealSense, to visualize and register a point cloud. -

- -

diff --git a/rviz/perception_example.rviz b/rviz/perception_example.rviz deleted file mode 100644 index a89a37b..0000000 --- a/rviz/perception_example.rviz +++ /dev/null @@ -1,409 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /RobotModel1/Links1/camera_bottom_screw_frame1 - Splitter Ratio: 0.5 - Tree Height: 1079 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_bottom_screw_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_color_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_color_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_infra1_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_infra1_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_infra2_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_infra2_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - laser: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_arm_l0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_arm_l1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_arm_l2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_arm_l3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_arm_l4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_aruco_inner_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_aruco_left_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_aruco_right_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_aruco_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_aruco_top_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_grasp_center: - Alpha: 1 - Show Axes: false - Show Trail: false - link_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_gripper_finger_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_gripper_finger_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_gripper_fingertip_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_gripper_fingertip_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_head: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_head_pan: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_head_tilt: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_lift: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_mast: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_wrist_yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - respeaker_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /camera/depth/color/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /camera/color/image_raw_upright_view - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/Camera - Enabled: false - Image Rendering: background and overlay - Image Topic: "" - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Visibility: - DepthCloud: true - Grid: true - Image: true - PointCloud2: true - RobotModel: true - Value: true - Zoom Factor: 1 - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: true - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: "" - Color Transformer: "" - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: "" - Depth Map Transport Hint: raw - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: "" - Queue Size: 5 - Selectable: true - Size (Pixels): 3 - Style: Flat Squares - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 3.6434335708618164 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.1298813819885254 - Y: -0.5344312787055969 - Z: 1.5416947603225708 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.23479780554771423 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.4154603481292725 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 1376 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000253000004c2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000022f000000c70000000000000000fb0000000a0049006d00610067006500000002920000026d0000001600fffffffb0000000c00430061006d0065007200610000000429000000d60000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006550000003efc0100000002fb0000000800540069006d0065010000000000000655000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003fc000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1621 - X: 864 - Y: 38 diff --git a/rviz_basics.md b/rviz_basics.md deleted file mode 100644 index 4b8ebdb..0000000 --- a/rviz_basics.md +++ /dev/null @@ -1,38 +0,0 @@ -## Visualizing with RViz - -You can utilize RViz to visualize Stretch's sensor information. To begin, run the stretch driver launch file. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.roslaunch -``` - -Then run the following command to bring up a simple RViz configuration of the Stretch robot. -```bash -# Terminal 2 -rosrun rviz rviz -d `rospack find stretch_core`/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 the *Add* button and include the *TF* type in 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). - - -## Running RViz and Gazebo (Simulation) -Let's bringup stretch in the willowgarage world from the [gazebo basics tutorial](gazebo_basics.md) and RViz by using the following command. - -```bash -roslaunch stretch_gazebo gazebo.launch world:=worlds/willowgarage.world rviz:=true -``` -the `rviz` flag will open an RViz window to visualize a variety of ROS topics. - -![image](images/willowgarage_with_rviz.png) - -Bringup the [keyboard teleop](teleoperating_stretch.md) to drive Stretch and observe its sensor input. - -**Next Tutorial:** [Navigation Stack](navigation_stack.md) diff --git a/src/avoider.py b/src/avoider.py deleted file mode 100755 index 5a70905..0000000 --- a/src/avoider.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy -from numpy import linspace, inf, tanh -from math import sin - -# 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 -from sensor_msgs.msg import LaserScan - -class Avoider: - """ - A class that implements both a LaserScan filter and base velocity control - for collision avoidance. - """ - def __init__(self): - """ - Function that initializes the subscriber, publisher, and marker features. - :param self: The self reference. - """ - # 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. - 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 - self.distance = 0.5 - - # Alocate 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 - self.twist.linear.y = 0.0 - self.twist.linear.z = 0.0 - self.twist.angular.x = 0.0 - self.twist.angular.y = 0.0 - self.twist.angular.z = 0.0 - - - # Called every time we get a LaserScan message from ROS - def set_speed(self,msg): - """ - Callback function to deal with incoming laserscan messages. - :param self: The self reference. - :param msg: The subscribed laserscan message. - - :publishes self.twist: Twist message. - """ - # 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)] - - # 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 - # and driving when it is close to the desired distance - 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) - -if __name__ == '__main__': - # Initialize the node, and call it "avoider" - rospy.init_node('avoider') - - # Instantiate he 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 - rospy.spin() diff --git a/src/effort_sensing.py b/src/effort_sensing.py deleted file mode 100755 index e3dcf52..0000000 --- a/src/effort_sensing.py +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy -import time -import actionlib -import os -import csv - -# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to -# control the Stretch robot. -from control_msgs.msg import FollowJointTrajectoryGoal - -# Import JointTrajectoryPoint from the trajectory_msgs package to define -# robot trajectories. -from trajectory_msgs.msg import JointTrajectoryPoint - -# We're going to subscribe to 64-bit integers, so we need to import the definition -# for them. -from sensor_msgs.msg import JointState - -# Import hello_misc script for handling trajecotry goals with an action client. -import hello_helpers.hello_misc as hm - -# Import datetime for naming the exported data files -from datetime import datetime - -class JointActuatorEffortSensor(hm.HelloNode): - """ - A class that sends multiple joint trajectory goals to a single joint. - """ - # Initialize the inhereted hm.Hellonode class. - def __init__(self): - """ - Function that initializes the subscriber,and other features. - :param self: The self reference. - """ - hm.HelloNode.__init__(self) - - # Set up a subscriber. We're going to subscribe to the topic "joint_states" - self.sub = rospy.Subscriber('joint_states', JointState, self.callback) - - # Create a list of joints - self.joints = ['joint_lift'] - - # Create an empty list for later storage. - self.joint_effort = [] - - # Create path to save effort and position values - self.save_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data' - - # Create boolean data type for conditional statements if you want to export effort data. - self.export_data = False - - - def callback(self, msg): - """ - Callback function to update and store JointState messages. - :param self: The self reference. - :param msg: The JointState message. - """ - # Store the joint messages for later use - self.joint_states = msg - - - def issue_command(self): - """ - Function that makes an action call and sends joint trajectory goals - to a single joint - :param self: The self reference. - """ - - # Set trajectory_goal as a FollowJointTrajectoryGoal and define - # the joint name. - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = self.joints - - # Provide desired positions for joint name. - point0 = JointTrajectoryPoint() - point0.positions = [0.9] - - # Set a list to the trajectory_goal.trajectory.points - trajectory_goal.trajectory.points = [point0] - - # 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. Also define the feedback and - # done callback function. - self.trajectory_client.send_goal(trajectory_goal, feedback_cb=self.feedback_callback, done_cb=self.done_callback) - rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - - def feedback_callback(self,feedback): - """ - The feedback_callback function deals with the incoming feedback messages - from the trajectory_client. Although, in this function, we do not use the - feedback information. - :param self: The self reference. - :param feedback: FollowJointTrajectoryActionFeedback message. - """ - # Conditional statement for replacement of joint names if wrist_extension - # is in the self.joint_names list. - if 'wrist_extension' in self.joints: - self.joints.remove('wrist_extension') - self.joints.append('joint_arm_l0') - - # create an empty list of current effort values that will be appended - # to the overall joint_effort list. - current_effort = [] - - # Use of forloop to parse the names of the requested joints list. - # The index() function returns the index at the first occurrence of - # the name of the requested joint in the self.joint_states.name list. - for joint in self.joints: - index = self.joint_states.name.index(joint) - current_effort.append(self.joint_states.effort[index]) - - # If the self.export_data boolean is false, then print the names and efforts - # of the joints in the terminal. - if not self.export_data: - # Print the joint position values to the terminal - print("name: " + str(self.joints)) - print("effort: " + str(current_effort)) - - # Else, append the current effort to the joint_effort list. - else: - self.joint_effort.append(current_effort) - - - def done_callback(self, status, result): - """ - The done_callback function will be called when the joint action is complete. - Within this function we export the data to a .txt file in the /stored_data directory. - :param self: The self reference. - :param status: status attribute from FollowJointTrajectoryActionResult message. - :param result: result attribute from FollowJointTrajectoryActionResult message. - """ - # Conditional statemets to notify whether the action succeeded or failed. - if status == actionlib.GoalStatus.SUCCEEDED: - rospy.loginfo('Suceeded') - else: - rospy.loginfo('Failed') - - # Conditional statement for exporting data. - if self.export_data: - # File name is the date and time the action was complete - file_name = datetime.now().strftime("%Y-%m-%d_%I:%M:%S-%p") - completeName = os.path.join(self.save_path, file_name) - - # Write the joint names and efforts to a .txt file. - with open(completeName, "w") as f: - writer = csv.writer(f) - writer.writerow(self.joints) - writer.writerows(self.joint_effort) - - - def main(self): - """ - Function that initiates the issue_command function. - :param self: The self reference. - """ - # 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, 'issue_command', 'issue_command', wait_for_first_pointcloud=False) - rospy.loginfo('issuing command...') - self.issue_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - # Declare object, node, from JointActuatorEffortSensor class - node = JointActuatorEffortSensor() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') diff --git a/src/joint_state_printer.py b/src/joint_state_printer.py deleted file mode 100755 index d19861c..0000000 --- a/src/joint_state_printer.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env python - -# Import ROS Python basic API and sys -import rospy -import sys - -# We're going to subscribe to 64-bit integers, so we need to import the definition -# for them -from sensor_msgs.msg import JointState - -class JointStatePublisher(): - """ - A class that prints the positions of desired joints in Stretch. - """ - - def __init__(self): - """ - Function that initializes the subsriber. - :param self: The self reference - """ - # Set up a subscriber. We're going to subscribe to the topic "joint_states" - self.sub = rospy.Subscriber('joint_states', JointState, self.callback) - - - def callback(self, msg): - """ - Callback function to deal with the incoming JointState messages. - :param self: The self reference. - :param msg: The JointState message. - """ - # Store the joint messages for later use - self.joint_states = msg - - - def print_states(self, joints): - """ - print_states function to deal with the incoming JointState messages. - :param self: The self reference. - :param joints: A list of joint names. - """ - # Create an empty list that will store the positions of the requested joints - joint_positions = [] - - # Use of forloop to parse the names of the requested joints list. - # The index() function returns the index at the first occurrence of - # the name of the requested joint in the self.joint_states.name list - for joint in joints: - index = self.joint_states.name.index(joint) - joint_positions.append(self.joint_states.position[index]) - - # Print the joint position values to the terminal - print("name: " + str(joints)) - print("position: " + str(joint_positions)) - - # Sends a signal to rospy to shutdown the ROS interfaces - rospy.signal_shutdown("done") - - # Exit the Python interpreter - sys.exit(0) - - - -if __name__ == '__main__': - # Initialize the node. - rospy.init_node('joint_state_printer', anonymous=True) - - # Declare object from JointStatePublisher class - JSP = JointStatePublisher() - - # Use the rospy.sleep() function to allow the class to initialize before - # requesting to publish joint_positions of desired joints (running the - # print_states() function) - rospy.sleep(.1) - - # Create a list of the joints and name them joints. These will be an argument - # for the print_states() function - joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3", "joint_wrist_yaw"] - # joints = ["joint_head_pan","joint_head_tilt", "joint_gripper_finger_left", "joint_gripper_finger_right"] - JSP.print_states(joints) - - # 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 deleted file mode 100755 index dd05400..0000000 --- a/src/marker.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/env python - -# Import rospy and sys -import rospy - -# Import the Marker message type from the visualization_msgs package. -from visualization_msgs.msg import Marker - -class Balloon(): - """ - A class that attaches a Sphere marker directly above the Stretch robot. - """ - def __init__(self): - """ - Function that initializes the marker's features. - :param self: The self reference. - """ - # Set up a publisher. We're going to publish on a topic called balloon - self.pub = 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.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 - - # Create log message - rospy.loginfo("Publishing the balloon topic. Use RViz to visualize.") - - - def publish_marker(self): - """ - Function that publishes the sphere marker - :param self: The self reference. - - :publishes self.marker: Marker message. - """ - # publisher the marker - self.pub.publish(self.marker) - - -if __name__ == '__main__': - # Initialize the node, as usual - rospy.init_node('marker') - - # Declare object from the Balloon class - balloon = Balloon() - - # Set a rate - rate = rospy.Rate(10) - - # Publish the marker at 10Hz - while not rospy.is_shutdown(): - balloon.publish_marker() - rate.sleep() diff --git a/src/move.py b/src/move.py deleted file mode 100755 index 62c8a07..0000000 --- a/src/move.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy - -# The Twist message is used to send velocities to the robot -from geometry_msgs.msg import Twist - -class Move: - """ - A class that sends Twist messages to move the Stretch robot foward. - """ - def __init__(self): - """ - Function that initializes the publisher. - :param self: The self reference - """ - # 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 - - - def move_forward(self): - """ - Function that publishes Twist messages - :param self: The self reference. - - :publishes command: Twist message. - """ - # 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.1 - 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.0 - - # Publish the Twist commands - self.pub.publish(command) - -if __name__ == '__main__': - # Initialize the node, and call it "move" - rospy.init_node('move') - - # Declare object from Move class - base_motion = Move() - - # 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) - - # 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(): - # 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() diff --git a/src/multipoint_command.py b/src/multipoint_command.py deleted file mode 100755 index 890502b..0000000 --- a/src/multipoint_command.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy -import time - -# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to -# control the Stretch robot -from control_msgs.msg 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): - """ - A class that sends multiple joint trajectory goals to the stretch robot. - """ - - # Initialize the inhereted hm.Hellonode class - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_multipoint_command(self): - """ - Function that makes an action call and sends multiple joint trajectory goals - to the joint_lift, wrist_extension, and joint_wrist_yaw. - :param self: The self reference. - """ - # 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 accelerations 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 = 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 list of goals = {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): - """ - Function that initiates the multipoint_command function. - :param self: The self reference. - """ - # 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) - - -if __name__ == '__main__': - try: - # Instantiate the MultiPointCommand class and execute the main() method - node = MultiPointCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') diff --git a/src/scan_filter.py b/src/scan_filter.py deleted file mode 100755 index f0bd870..0000000 --- a/src/scan_filter.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy -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: - """ - A class that implements a LaserScan filter that removes all of the points. - that are not infront of the robot. - """ - 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 - - # 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) - - # Create log message - rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.") - - - def callback(self,msg): - """ - Callback function to deal with incoming laserscan messages. - :param self: The self reference. - :param msg: The subscribed laserscan message. - - :publishes msg: updated laserscan message. - """ - # 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) - -if __name__ == '__main__': - # Initialize the node, and call it "scan_filter" - rospy.init_node('scan_filter') - - # Instantiate the ScanFilter class - 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 - rospy.spin() diff --git a/src/single_joint_actuator.py b/src/single_joint_actuator.py deleted file mode 100755 index 938c967..0000000 --- a/src/single_joint_actuator.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy -import time - -# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to -# control the Stretch robot. -from control_msgs.msg 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 SingleJointActuator(hm.HelloNode): - """ - A class that sends multiple joint trajectory goals to a single joint. - """ - # Initialize the inhereted hm.Hellonode class - def __init__(self): - hm.HelloNode.__init__(self) - - def issue_command(self): - """ - Function that makes an action call and sends joint trajectory goals - to a single joint - :param self: The self reference. - """ - # Here is a list of joints and their recommended position limits: - ############################# Joint limits ############################# - # joint_lift: lower_limit = 0.15, upper_limit = 1.10 # in meters - # wrist_extension: lower_limit = 0.00, upper_limit = 0.50 # in meters - # joint_wrist_yaw: lower_limit = -1.75, upper_limit = 4.00 # in radians - # joint_head_pan: lower_limit = -2.80, upper_limit = 2.90 # in radians - # joint_head_tilt: lower_limit = -1.60, upper_limit = 0.40 # in radians - # joint_gripper_finger_left: lower_limit = -0.35, upper_limit = 0.165 # in radians - ######################################################################## - - # Set trajectory_goal as a FollowJointTrajectoryGoal and define - # the joint name - trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.trajectory.joint_names = ['joint_head_pan'] - - # Provide desired positions for joint name. - # Set positions for the following 5 trajectory points - point0 = JointTrajectoryPoint() - point0.positions = [0.65] - - # point1 = JointTrajectoryPoint() - # point1.positions = [-1.50] - - # Then trajectory_goal.trajectory.points is set as a list of the joint - # trajectory points - trajectory_goal.trajectory.points = [point0]#, point1] - - # 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 goal = {0}'.format(trajectory_goal)) - self.trajectory_client.wait_for_result() - - def main(self): - """ - Function that initiates the issue_command function. - :param self: The self reference. - """ - # 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, 'issue_command', 'issue_command', wait_for_first_pointcloud=False) - rospy.loginfo('issuing command...') - self.issue_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - # Declare object from the SingleJointActuator class. Then execute the - # main() method/function - node = SingleJointActuator() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') diff --git a/src/stored_data_plotter.py b/src/stored_data_plotter.py deleted file mode 100755 index ad638eb..0000000 --- a/src/stored_data_plotter.py +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python - -# Import needed python packages -import numpy as np -import os -import csv -import pandas as pd -import matplotlib.pyplot as plt -from matplotlib import animation - -class Plotter(): - """ - Class the plots stored data from the effort sensing node. - """ - def __init__(self,animate=False): - """ - Function that stores the dataframe. - :param self: The self reference. - :param animate: Boolean to determine animation plotting. - """ - # Create path to save effort and position values - dir_path = '/home/hello-robot/catkin_ws/src/stretch_ros_tutorials/stored_data' - - ####################### Copy the file name here! ####################### - file_name = '2022-06-30_11:26:20-AM' - - # Complete name of directory to pull from - self.completeName = os.path.join(dir_path, file_name) - - # Store dataframe - self.data = pd.read_csv(self.completeName) - - # Create empty list for animation - self.y_anim = [] - - # Set self.animate to boolean argument - self.animate = animate - - def plot_data(self): - """ - Function that plots dataframe - :param self: The self reference. - """ - # Utililze a forloop to print each joint's effort - for joint in self.data.columns: - - # Create figure, labels, and title - fig = plt.figure() - plt.title(joint + ' Effort Sensing') - plt.ylabel('Effort') - plt.xlabel('Data Points') - - # Conditional statement for animation plotting - if self.animate: - self.effort = self.data[joint] - frames = len(self.effort)-1 - anim = animation.FuncAnimation(fig=fig,func=self.plot_animate, repeat=False,blit=False,frames=frames, interval =75) - plt.show() - - ## If you want to save a video, make sure to comment out plt.show(), - ## right before this comment. - # save_path = str(self.completeName + '.mp4') - # anim.save(save_path, writer=animation.FFMpegWriter(fps=10)) - - # Reset y_anim for the next joint effort animation - del self.y_anim[:] - - - # Conditional statement for regular plotting (No animation) - else: - self.data[joint].plot(kind='line') - # save_path = str(self.completeName + '.png') - # plt.savefig(save_path, bbox_inches='tight') - plt.show() - - - def plot_animate(self,i): - """ - Function that plots every increment of the dataframe. - :param self: The self reference. - :param i: index value. - """ - # Append self.effort values for given joint. - self.y_anim.append(self.effort.values[i]) - plt.plot(self.y_anim, color='blue') - - -if __name__ == '__main__': - viz = Plotter(animate=True) - viz.plot_data() diff --git a/src/stow_command.py b/src/stow_command.py deleted file mode 100755 index cefd3c5..0000000 --- a/src/stow_command.py +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/env python - -# Every python controller needs these lines -import rospy -import time - -# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to -# control the Stretch robot -from control_msgs.msg 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): - ''' - A class that sends a joint trajectory goal to stow the Stretch's arm. - ''' - - # Initialize the inhereted hm.Hellonode class - def __init__(self): - hm.HelloNode.__init__(self) - - - def issue_stow_command(self): - ''' - Function that makes an action call and sends stow postion goal. - :param self: The self reference. - ''' - # Set stow_point as a JointTrajectoryPoint() - stow_point = JointTrajectoryPoint() - stow_point.time_from_start = rospy.Duration(0.000) - - # 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 = 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): - ''' - Function that initiates stow_command function. - :param self: The self reference. - ''' - # 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...') - self.issue_stow_command() - time.sleep(2) - - -if __name__ == '__main__': - try: - # Declare object from StowCommand class then execute the main() method - node = StowCommand() - node.main() - except KeyboardInterrupt: - rospy.loginfo('interrupt received, so shutting down') diff --git a/stored_data/2022-06-30_11:26:20-AM b/stored_data/2022-06-30_11:26:20-AM deleted file mode 100644 index 6dc02d0..0000000 --- a/stored_data/2022-06-30_11:26:20-AM +++ /dev/null @@ -1,54 +0,0 @@ -joint_lift -30.90921974182129 -30.90921974182129 -30.90921974182129 -35.687774658203125 -46.455074310302734 -53.12174606323242 -74.74340057373047 -74.74340057373047 -78.7086181640625 -81.2074203491211 -83.81580352783203 -84.66924285888672 -86.44051361083984 -86.29670715332031 -84.92870330810547 -85.26908111572266 -83.41191101074219 -83.6566162109375 -82.04027557373047 -82.04027557373047 -81.03909301757812 -81.51132202148438 -81.67819213867188 -82.48448944091797 -82.48580169677734 -83.62725067138672 -84.09523010253906 -84.09523010253906 -85.30419158935547 -84.31338500976562 -85.0532455444336 -84.40817260742188 -82.8752212524414 -82.47306823730469 -83.45247650146484 -83.28417205810547 -84.3813247680664 -84.37670135498047 -85.32344818115234 -84.87259674072266 -86.09488677978516 -85.2378921508789 -85.44424438476562 -85.44424438476562 -84.78129577636719 -84.24555206298828 -84.24555206298828 -83.79412078857422 -84.11444091796875 -83.61358642578125 -83.63019561767578 -83.30967712402344 -83.30967712402344 diff --git a/stored_data/2022-06-30_11:26:20-AM.mp4 b/stored_data/2022-06-30_11:26:20-AM.mp4 deleted file mode 100644 index bdb224b..0000000 Binary files a/stored_data/2022-06-30_11:26:20-AM.mp4 and /dev/null differ diff --git a/stored_data/2022-06-30_11:26:20-AM.png b/stored_data/2022-06-30_11:26:20-AM.png deleted file mode 100644 index 83e24c0..0000000 Binary files a/stored_data/2022-06-30_11:26:20-AM.png and /dev/null differ diff --git a/docs/tool_change_tutorial.md b/stretch_body/tool_change_tutorial.md similarity index 100% rename from docs/tool_change_tutorial.md rename to stretch_body/tool_change_tutorial.md diff --git a/teleoperating_stretch.md b/teleoperating_stretch.md deleted file mode 100644 index 633775f..0000000 --- a/teleoperating_stretch.md +++ /dev/null @@ -1,164 +0,0 @@ -## Teleoperating Stretch - -### 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: - -* Remove the 'trunk' cover and power on the robot -Wait for about 45 seconds. You will hear the Ubuntu startup sound, followed by two beeps (indicating the demo is running). -* Hit the Connect button on the controller. The upper two LEDs of the ring will illuminate. -* Hit the Home Robot button. Stretch will go through its homing calibration routine. -* **Note**: make sure the space around the robot is clear before running the Home function - -You're ready to go! A few things to try: - -* Hit the Stow Robot button. The robot will assume the stow pose. -* Practice driving the robot around. -* Pull the Fast Base trigger while driving. When stowed, it will make Stretch drive faster -* Manually stop the arm or lift from moving to make it stop upon contact. -* Try picking up your cellphone from the floor -* Try grasping cup from a counter top -* Try delivering an object to a person - -If you're done, hold down the Shutdown PC button for 2 seconds. This will cause the PC to turn off. You can then power down the robot. - -### Keyboard Teleoperating: Mobile Base - -Begin by running the following command in your terminal: - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` - -To teleoperate a Stretch's mobile base with the keyboard, you first need to switch the mode to *nagivation* for the robot to receive *Twist* messages. This is done using a rosservice call in a new terminal. In the same terminal run the teleop_twist_keyboard node with the argument remapping the *cmd_vel* topic name to *stretch/cmd_vel*. - -```bash -# Terminal 2 -rosservice call /switch_to_navigation_mode -rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=stretch/cmd_vel -``` - -Below are the keyboard commands that allow a user to move Stretch's base. -``` -Reading from the keyboard and Publishing to Twist! ---------------------------- -Moving around: - u i o - j k l - m , . - -For Holonomic mode (strafing), hold down the shift key: ---------------------------- - U I O - J K L - M < > - -t : up (+z) -b : down (-z) - -anything else : stop - -q/z : increase/decrease max speeds by 10% -w/x : increase/decrease only linear speed by 10% -e/c : increase/decrease only angular speed by 10% - -CTRL-C to quit - -currently: speed 0.5 turn 1.0 - -``` -To stop the node from sending twist messages, type **Ctrl** + **c**. - -### Create a node for Mobile Base Teleoperating -To move Stretch's mobile base using a python script, please look at [example 1](example_1.md) for reference. - -### Keyboard Teleoperating: Full Body - -For full body teleoperation with the keyboard, you first need to run the `stretch_driver.launch` in a terminal. - -```bash -# Terminal 1 -roslaunch stretch_core stretch_driver.launch -``` -Then in a new terminal, type the following command - -```bash -# Terminal 2 -rosrun stretch_core keyboard_teleop -``` - -Below are the keyboard commands that allow a user to control all of Stretch's joints. -``` ----------- KEYBOARD TELEOP MENU ----------- - - i HEAD UP - j HEAD LEFT l HEAD RIGHT - , HEAD DOWN - - - 7 BASE ROTATE LEFT 9 BASE ROTATE RIGHT - home page-up - - - 8 LIFT UP - up-arrow - 4 BASE FORWARD 6 BASE BACK - left-arrow right-arrow - 2 LIFT DOWN - down-arrow - - - w ARM OUT - a WRIST FORWARD d WRIST BACK - x ARM IN - - - 5 GRIPPER CLOSE - 0 GRIPPER OPEN - - step size: b BIG, m MEDIUM, s SMALL - - q QUIT - -------------------------------------------- - -``` -To stop the node from sending twist messages, type **Ctrl** + **c**. - - -## Teleoperating in Gazebo - - -### Keyboard Teleoperating: Mobile Base -For keyboard teleoperation of the Stretch's mobile base, first [startup Stretch in simulation](gazebo_basics.md). Then run the following command in a new terminal. - -```bash -# Terminal 1 -roslaunch stretch_gazebo gazebo.launch -``` - -In a new terminal, type the following - -```bash -# Terminal 2 -roslaunch stretch_gazebo teleop_keyboard.launch -``` -The same keyboard commands will be presented to a user to move the robot. - -### Xbox Controller Teleoperating -An alternative for robot base teleoperation is to use an Xbox controller. Stop the keyboard teleoperation node by typing **Ctrl** + **c** in the terminal where the command was executed. Then connect the Xbox controller device to your local machine and run the following command. - -```bash -# Terminal 2 -roslaunch stretch_gazebo teleop_joy.launch -``` -Note that the teleop_twist_joy package has a deadman switch by default which disables the drive commands to be published unless pressed. For a Logitech F310 joystick, this button is A. - -**Next Tutorial:** [Internal State of Stretch](internal_state_of_stretch.md)