The aim of example 3 is to combine the two previous examples and have Stretch utilize its laser scan data to avoid collision with objects as it drives forward.
Begin by running `roscore` in a terminal. Then set the ros parameter to *navigation* mode by running the following commands in a new terminal.
Begin by running the following commands in a new terminal.
```bash
# Terminal 2
rosparam set /stretch_driver/mode "navigation"
# Terminal 1
roslaunch stretch_core stretch_driver.launch
```
Then in a new terminal type the following to activate the LiDAR sensor.
```bash
# Terminal 3
# Terminal 2
roslaunch stretch_core rplidar.launch
```
To activate the avoider node, type the following in a new terminal.
To set *navigation* mode and to activate the avoider node, type the following in a new terminal.
```bash
# Terminal 4
# Terminal 3
rosservice call /switch_to_navigation_mode
cd catkin_ws/src/stretch_ros_tutorials/src/
python3 avoider.py
python avoider.py
```
To stop the node from sending twist messages, type **Ctrl** + **c** in the terminal running the avoider node.
<!-- 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). -->
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).
Begin by running `roscore` in a terminal. Then set the ros parameter to *position* mode by running the following commands in a new terminal.
Begin by running the following command in the terminal in a terminal.
```bash
# Terminal 2
rosparam set /stretch_driver/mode "position"
# Terminal 1
roslaunch stretch_core stretch_driver.launch
```
In a new terminal type the following commands.
Switch the mode to *manipulation* mode using a rosservice call. Then run the single effort sensing node.
```bash
# Terminal 3
# 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.
<!-- 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: -->
This will send a `FollowJointTrajectory` command to move Stretch's arm or head while also printing the effort of the lift.
### The Code
```python
@ -42,12 +32,12 @@ 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
from datetime import datetime
class JointActuatorEffortSensor(hm.HelloNode):
"""
@ -158,7 +148,7 @@ if __name__ == '__main__':
### The Code Explained
Now let's break the code down.
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
@ -167,7 +157,151 @@ Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at
```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.
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.
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.
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.
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.
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
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.
Alan G. Sanchez
2 years ago