Skip to content

Changes

Page history
Update lab2, clarification rviz instructions authored by Tommy Persson's avatar Tommy Persson
Hide whitespace changes
Inline Side-by-side
lab2.md
View page @ c8b80938
Lab 2: Navigation
=================
In this lab, you will start programming your first ROS node.
**Remainder!**
```start_ros``` needs to be used to start ```bash``` terminal with ROS setup when you open a new Terminal window.
**This has to be run always before any commands discussed below when using new Terminal window, otherwise ROS commands will not work.**
Setup the Environment
---------------------
To setup your environment:
```bash
mkdir -p ~/TDDE05/ros2_ws/src
cd ~/TDDE05/ros2_ws/
tdde05-build
```
The `tdde05-build` command runs `colcon build`, with the advantage of setting symlinks for python scripts. And also can `tdde05-build` be run from any directory, while `colcon build` needs to be run from `~/TDDE05/ros2_ws/`.
The command `tdde05-clean` can also be used to clean a build directory in case of problems.
**It is strongly advised that you use the exact directory structure.** A number of scripts are provided by the course, and we expect you to follow that structure.
In ```~/TDDE05/ros2_ws/```, you will now have four directories
* ```build```: used to build your modules
* ```install```: directory where your modules are installed
* ```log```: directory with building logs
* ```src```: the directory with the source for your modules
Setup GIT Repository
--------------------
<details>
<summary><span style="font-weight:bold;">Click me - Your GIT repository and adding SSH Key!</span></summary>
### Your GIT repository name:
The course uses automatically generated GIT repositories for storing your lab assignments source code.
Make sure you are registered on Webreg, so that the repository is created. The assigned name of the repository follows ```gitlab.liu.se:tdde05-2025/tdde05-labs-air-XX.git``` pattern, where ```XX``` is your group number in WebReg.
### SSH Key - if you do not have one yet
Working with GitLab requires setting up an SSH Key. If you have not done it in previous corses
1. Click ```Add new key``` in [SSH Keys section of your profile](https://gitlab.liu.se/-/user_settings/ssh_keys)
2. Follow the instructions [HERE](https://gitlab.liu.se/help/user/ssh.md) to generate the key and add it to your GitLab profile.
</details>
</br>
You should go to the source directory in your environment:
```bash
cd ~/TDDE05/ros2_ws/src/
mkdir labs
cd labs
git init
git remote add origin git@gitlab.liu.se:tdde05-2025/tdde05-labs-air-XX.git
git push --set-upstream origin master
```
Using Visual Studio Code as IDE
--------------------
You can use Visual Studio Code to work with your files. It's important to start the VSCode in the ROS environment, that is, execute first ```start_ros``` before executing ```code``` command. Once VSCode has started ```Open Folder``` and select ```~/TDDE05/ros2_ws``` folder.
If you are using Python, no additional steps are required.
If you are using C++, you can add a custom build command by editing the default built task (```Configure Default Built Task...``` in ```Terminal``` menu bar). Use the following settings:
```bash
{
"version": "2.0.0",
"tasks": [
{
"type": "shell",
"label": "C/C++: clang-14 build active file",
"command": "tdde05-build",
"options": {
"cwd": "${fileDirname}"
},
"problemMatcher": [
"$gcc"
],
"group": {
"kind": "build",
"isDefault": true
},
"detail": "compiler: /usr/bin/clang-14"
}
]
}
```
<details>
<summary><span style="font-weight:bold;">Click me - VSCode screenshot</span></summary>
![VSCode](uploads/code-python.jpg)
</details>
Creating a Package for Screen and Rviz
------------------------------------
We will create a package ```air_lab_common``` to add common files among all the labs, such as the screen file to start commands and the Rviz configuration.
```bash
cd ~/TDDE05/ros2_ws/src/labs
ros2 pkg create --build-type ament_python air_lab_common
```
Then we will add the screen and Rviz configuration:
```bash
cd ~/TDDE05/ros2_ws/src/labs/air_lab_common
mkdir screen
mkdir rviz
```
Then copy in ```screen``` and ```rviz``` the files you created in *lab1*. You will need to modify the ```setup.py``` file to include the following:
```python
import glob
# And modify data_files line so that it looks like this:
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + "/screen", glob.glob('screen/*')),
],
```
Before we can we build, we need to have at least one commit in git:
```bash
cd ~/TDDE05/ros2_ws/src/labs/air_lab_common
git add .
git commit -m "import air_lab_common"
```
You can now run ```tdde05-build``` to install your files. After which you need to reload the configuration either by:
```bash
source $HOME/TDDE05/ros2_ws/install/local_setup.bash
```
or starting a new shell with:
```bash
start_ros
```
Now, you can start your screen file with:
```bash
ros2screen air_lab_common [name of screen file]
```
We also would like to load Rviz configuration automatically. To do so you can use ```-d``` flag, and modify your screen file to start Rviz with the following:
```bash
rviz2 -d $HOME/TDDE05/ros2_ws/src/labs/air_lab_common/rviz/[name of your rviz config]
```
Creating a Package for Lab2
-------------------------
To create a Python package with a default node:
```bash
cd ~/TDDE05/ros2_ws/src/labs
ros2 pkg create --build-type ament_python --node-name lab2_node air_lab2
```
To create a C++ package with a default node (only use C++ if you are knowledgeable in C++, otherwise, use Python):
```bash
cd ~/TDDE05/ros2_ws/src/labs
ros2 pkg create --build-type ament_cmake --node-name lab2_node air_lab2
```
You can now run the build command:
```bash
tdde05-build
```
Then, you need to source the environment with:
```bash
source $HOME/TDDE05/ros2_ws/install/local_setup.bash
```
or start a new shell with:
```bash
start_ros
```
You can then run your node with the following command:
```bash
ros2 run air_lab2 lab2_node
```
Publishing on a Topic
---------------------
At this point, your node is not doing anything interesting. First, we will make our node send a message on a topic. We will make it send a velocity on ```/cmd_vel```.
Before starting, you should study the [official Python tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html) or the [official C++ tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
Modify the publisher example to instead publish a ```geometry_msgs/msg/Twist``` (you can import it from ```import geometry_msgs.msg```) with a linear velocity of ```0.1``` and angular velocity of ```0.05```. We strongly advise that you follow an incremental approach:
* Copy/paste the publisher example from the respective tutorials into your ```lab2_node.py``` or ```lab2_node.cpp```
* Make sure you understand what is happening, if you have doubts, ask questions to your lab assistant
* Build your project with ```tdde05-build``` and then use ```ros2 run ...``` to start your node. Alternatively, for Python, you can start the script directly from the source directory.
* Check that it runs properly, you can use ```ros2 topic echo ...``` or ```rqt``` to check that a message is published by the node, it should be a string.
* Modify the script to publish a ```geometry_msgs/msg/Twist``` on ```/cmd_vel``` with a linear velocity of ```0.1``` and angular velocity of ```0.05```.
The documentation for ```geometry_msgs/Twist``` can be found online at [geometry_msgs/Twist](https://docs.ros2.org/galactic/api/geometry_msgs/msg/Twist.html) or by running ```ros2 msg info geometry_msgs/Twist```.
Listening to a Topic
-----------------
In the previous part, we created a node that publishes on a topic and makes your robot move, but it never stops. In this part, we will listen to the ```/odom``` topic and use that information to stop when the robot has moved more than 1.0 meters away from its start position.
Check in the Python or C++ tutorial to see how you can subscribe to a topic. You will have to somehow combine together the publisher and subscriber classes. We strongly advise that you follow an incremental approach:
* Add a subscriber to your ```lab2_node```. **You should have a single class**, so add the `subscribe` and `callback` to your existing class. This is different from the ROS tutorials where the `subscribe` and `callback` are in different classes. By default, the `spin` function is blocking and accepts a single `node` as an argument.
* Add a printout in the callback to check that you received a message.
* Modify your program so that it exits once the robot has moved by 1.0 meters.
Command Line Parameters
----------
For now, we have hard-coded parameters, such as the velocity and the distance. To make our node more effective, we should set those as parameters on the command line.
So that we can run our program this way:
```bash
ros2 run air_lab2 lab2_node --ros-args -p linear:=0.5 -p angular:=0.1 -p distance:=0.4
```
Before starting, you should study the [official Python tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-Python.html) or the [official C++ tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html).
Create a ```linear```, ```angular``` and ```distance``` parameter.
Navigation and Path Planning
----------------------------
The turtlebot4 includes a motion planner that can navigate and avoid obstacles. This is implemented as an action that can be started from the terminal with the following command:
```bash
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 6.0, y: 2.0, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"
```
It can also be started from RViz. Before we start, you should add the following visualizations (similarly to Lab1, two types should be used, i.e., ```Map``` and ```Path```):
* ```/global_costmap/costmap``` and change the colorscheme to costmap
* ```/local_costmap/costmap``` and change the colorscheme to costmap
* ```/plan/```
* ```/local_plan/``` and change the color to red
If you run the ```NavigateToPose``` action, you should see the following in ```Rviz```:
![rviz_path_planning](uploads/1276ecebbfd573d82fd7bd64ae3c076f/rviz_path_planning.png)
We can also send ```NavigateToPose``` requests directly from ```Rviz```. To do so we need to add the following:
* In ```Panels``` menu, select ```Add new panel``` and then ```Navigation 2```
* In the toolbar, click on the plus, then select ```GoalTool``` under ```nav2_rviz_plugin```
Now you can select ```Nav2 goal``` tool and click in Rviz to make your robot move.
![rviz2_navigation_2](uploads/6b7d4787d3ff24dfb3f94fed12b10bab/rviz2_navigation_2.png)
Exploration with a Random Pattern
---------------------------------
We will now create a new node that will send the robot to a random location to explore the map. We will need to create a new node in the ```air_lab2``` module.
### Python
Create a new file in ```$HOME/TDDE05/ros2_ws/src/labs/air_lab2/air_lab2``` called ```random_exploration.py```. This will be your new node. You need to modify your ```setup.py``` so that it contains the following change to ```entry_points```:
```python
entry_points={
'console_scripts': [
'lab2_node = air_lab2.lab2_node:main',
'random_exploration = air_lab2.random_exploration:main'
],
},
```
We will use the ```NavigateToPose``` action. You can check its type with:
```bash
ros2 interface show nav2_msgs/action/NavigateToPose
```
Before starting the implementation, study the [official action tutorial](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html).
We already modified the tutorial, so that you can use the following in ```random_exploration.py``` to go to a single location:
```python
import rclpy
import math
from rclpy.action import ActionClient
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
class RandomExploration(Node):
def __init__(self):
super().__init__('random_exploration')
self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
def send_goal(self, x, y, angle):
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = "map"
goal_msg.pose.pose.position.x = x
goal_msg.pose.pose.position.y = y
goal_msg.pose.pose.orientation.w = math.cos(angle/2)
goal_msg.pose.pose.orientation.z = math.sin(angle/2)
self._action_client.wait_for_server()
self._send_goal_future = self._action_client.send_goal_async(goal_msg, self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
self._goal_handle = future.result()
if not self._goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = self._goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
rclpy.shutdown()
def feedback_callback(self, feedback_msg):
print(feedback_msg.feedback)
self.get_logger().info('Received feedback: {0}'.format(feedback_msg.feedback.navigation_time))
if feedback_msg.feedback.navigation_time.sec > 60:
self._goal_handle.cancel_goal_async()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
action_client = RandomExploration()
action_client.send_goal(10.0, 5.0, 1.0)
rclpy.spin(action_client)
if __name__ == '__main__':
main()
```
Extend that class so that you keep going to a new random location, either when the robot has reached its current location or when it is failing to reach it. Instead of basing yourself on time, you should check using feedback if the robot is moving. There are at least two approaches to solving that problem. One is much easier to implement than the other, so look carefully at the message structure.
### C++
Create a new file in ```$HOME/TDDE05/ros2_ws/src/labs/air_lab2/src``` called ```random_exploration.cpp```. This will be your new node. You need to modify your ```CMakeLists.txt```, to duplicate what is available for ```lab2_node```.
We will use the ```NavigateToPose``` action. You can check its type with:
```bash
ros2 interface show nav2_msgs/action/NavigateToPose
```
Before starting the implementation, study the [official action tutorial](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html).
We already modified the tutorial, so that you can use the following in ```random_exploration.py``` to go to a single location:
```c++
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
class RandomExploration : public rclcpp::Node
{
public:
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
explicit RandomExploration()
: Node("random_exploration")
{
this->client_ptr_ = rclcpp_action::create_client<NavigateToPose>(
this,
"navigate_to_pose");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&RandomExploration::send_goal, this));
}
void send_goal()
{
double x = 10.0;
double y = 5.0;
double angle = 1.0;
auto goal_msg = NavigateToPose::Goal();
goal_msg.pose.header.frame_id = "map";
goal_msg.pose.pose.position.x = x;
goal_msg.pose.pose.position.y = y;
goal_msg.pose.pose.orientation.w = std::cos(angle/2);
goal_msg.pose.pose.orientation.z = std::sin(angle/2);
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&RandomExploration::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&RandomExploration::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&RandomExploration::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
std::thread cancel_thread_;
void goal_response_callback(std::shared_future<GoalHandleNavigateToPose::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleNavigateToPose::SharedPtr goal_handle,
const std::shared_ptr<const NavigateToPose::Feedback> feedback)
{
if(feedback->navigation_time.sec>60)
{
//FIXME as of ROS Galactic, it looks like async_cancel_goal is not working properly
// current work around is to cancel all goals.
//client_ptr_->async_cancel_goal(goal_handle);
client_ptr_->async_cancel_all_goals();
RCLCPP_INFO(this->get_logger(), "Timeout");
rclcpp::shutdown();
}
}
void result_callback(const GoalHandleNavigateToPose::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Goal was succeeded");
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
rclcpp::shutdown();
}
}; // class RandomExploration
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<RandomExploration>());
rclcpp::shutdown();
return 0;
}
```
Extend that class so that you keep going to a new random location, either when the robot has reached its current location or when it is failing to reach it. Instead of basing yourself on time, you should check using feedback if the robot is moving. There are at least two approaches to solving that problem. One is much easier to implement than the other, so look carefully at the message structure.
Demonstration
--------------
* run your lab2_node and random_exploration nodes
* show your screen file and that the map is loaded correctly
Lab 2: Navigation
=================
In this lab, you will start programming your first ROS node.
**Remainder!**
```start_ros``` needs to be used to start ```bash``` terminal with ROS setup when you open a new Terminal window.
**This has to be run always before any commands discussed below when using new Terminal window, otherwise ROS commands will not work.**
Setup the Environment
---------------------
To setup your environment:
```bash
mkdir -p ~/TDDE05/ros2_ws/src
cd ~/TDDE05/ros2_ws/
tdde05-build
```
The `tdde05-build` command runs `colcon build`, with the advantage of setting symlinks for python scripts. And also can `tdde05-build` be run from any directory, while `colcon build` needs to be run from `~/TDDE05/ros2_ws/`.
The command `tdde05-clean` can also be used to clean a build directory in case of problems.
**It is strongly advised that you use the exact directory structure.** A number of scripts are provided by the course, and we expect you to follow that structure.
In ```~/TDDE05/ros2_ws/```, you will now have four directories
* ```build```: used to build your modules
* ```install```: directory where your modules are installed
* ```log```: directory with building logs
* ```src```: the directory with the source for your modules
Setup GIT Repository
--------------------
<details>
<summary><span style="font-weight:bold;">Click me - Your GIT repository and adding SSH Key!</span></summary>
### Your GIT repository name:
The course uses automatically generated GIT repositories for storing your lab assignments source code.
Make sure you are registered on Webreg, so that the repository is created. The assigned name of the repository follows ```gitlab.liu.se:tdde05-2025/tdde05-labs-air-XX.git``` pattern, where ```XX``` is your group number in WebReg.
### SSH Key - if you do not have one yet
Working with GitLab requires setting up an SSH Key. If you have not done it in previous corses
1. Click ```Add new key``` in [SSH Keys section of your profile](https://gitlab.liu.se/-/user_settings/ssh_keys)
2. Follow the instructions [HERE](https://gitlab.liu.se/help/user/ssh.md) to generate the key and add it to your GitLab profile.
</details>
</br>
You should go to the source directory in your environment:
```bash
cd ~/TDDE05/ros2_ws/src/
mkdir labs
cd labs
git init
git remote add origin git@gitlab.liu.se:tdde05-2025/tdde05-labs-air-XX.git
git push --set-upstream origin master
```
Using Visual Studio Code as IDE
--------------------
You can use Visual Studio Code to work with your files. It's important to start the VSCode in the ROS environment, that is, execute first ```start_ros``` before executing ```code``` command. Once VSCode has started ```Open Folder``` and select ```~/TDDE05/ros2_ws``` folder.
If you are using Python, no additional steps are required.
If you are using C++, you can add a custom build command by editing the default built task (```Configure Default Built Task...``` in ```Terminal``` menu bar). Use the following settings:
```bash
{
"version": "2.0.0",
"tasks": [
{
"type": "shell",
"label": "C/C++: clang-14 build active file",
"command": "tdde05-build",
"options": {
"cwd": "${fileDirname}"
},
"problemMatcher": [
"$gcc"
],
"group": {
"kind": "build",
"isDefault": true
},
"detail": "compiler: /usr/bin/clang-14"
}
]
}
```
<details>
<summary><span style="font-weight:bold;">Click me - VSCode screenshot</span></summary>
![VSCode](uploads/code-python.jpg)
</details>
Creating a Package for Screen and Rviz
------------------------------------
We will create a package ```air_lab_common``` to add common files among all the labs, such as the screen file to start commands and the Rviz configuration.
```bash
cd ~/TDDE05/ros2_ws/src/labs
ros2 pkg create --build-type ament_python air_lab_common
```
Then we will add the screen and Rviz configuration:
```bash
cd ~/TDDE05/ros2_ws/src/labs/air_lab_common
mkdir screen
mkdir rviz
```
Then copy in ```screen``` and ```rviz``` the files you created in *lab1*. You will need to modify the ```setup.py``` file to include the following:
```python
import glob
# And modify data_files line so that it looks like this:
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + "/screen", glob.glob('screen/*')),
],
```
Before we can we build, we need to have at least one commit in git:
```bash
cd ~/TDDE05/ros2_ws/src/labs/air_lab_common
git add .
git commit -m "import air_lab_common"
```
You can now run ```tdde05-build``` to install your files. After which you need to reload the configuration either by:
```bash
source $HOME/TDDE05/ros2_ws/install/local_setup.bash
```
or starting a new shell with:
```bash
start_ros
```
Now, you can start your screen file with:
```bash
ros2screen air_lab_common [name of screen file]
```
We also would like to load Rviz configuration automatically. To do so you can use ```-d``` flag, and modify your screen file to start Rviz with the following:
```bash
rviz2 -d $HOME/TDDE05/ros2_ws/src/labs/air_lab_common/rviz/[name of your rviz config]
```
Creating a Package for Lab2
-------------------------
To create a Python package with a default node:
```bash
cd ~/TDDE05/ros2_ws/src/labs
ros2 pkg create --build-type ament_python --node-name lab2_node air_lab2
```
To create a C++ package with a default node (only use C++ if you are knowledgeable in C++, otherwise, use Python):
```bash
cd ~/TDDE05/ros2_ws/src/labs
ros2 pkg create --build-type ament_cmake --node-name lab2_node air_lab2
```
You can now run the build command:
```bash
tdde05-build
```
Then, you need to source the environment with:
```bash
source $HOME/TDDE05/ros2_ws/install/local_setup.bash
```
or start a new shell with:
```bash
start_ros
```
You can then run your node with the following command:
```bash
ros2 run air_lab2 lab2_node
```
Publishing on a Topic
---------------------
At this point, your node is not doing anything interesting. First, we will make our node send a message on a topic. We will make it send a velocity on ```/cmd_vel```.
Before starting, you should study the [official Python tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html) or the [official C++ tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html).
Modify the publisher example to instead publish a ```geometry_msgs/msg/Twist``` (you can import it from ```import geometry_msgs.msg```) with a linear velocity of ```0.1``` and angular velocity of ```0.05```. We strongly advise that you follow an incremental approach:
* Copy/paste the publisher example from the respective tutorials into your ```lab2_node.py``` or ```lab2_node.cpp```
* Make sure you understand what is happening, if you have doubts, ask questions to your lab assistant
* Build your project with ```tdde05-build``` and then use ```ros2 run ...``` to start your node. Alternatively, for Python, you can start the script directly from the source directory.
* Check that it runs properly, you can use ```ros2 topic echo ...``` or ```rqt``` to check that a message is published by the node, it should be a string.
* Modify the script to publish a ```geometry_msgs/msg/Twist``` on ```/cmd_vel``` with a linear velocity of ```0.1``` and angular velocity of ```0.05```.
The documentation for ```geometry_msgs/Twist``` can be found online at [geometry_msgs/Twist](https://docs.ros2.org/galactic/api/geometry_msgs/msg/Twist.html) or by running ```ros2 msg info geometry_msgs/Twist```.
Listening to a Topic
-----------------
In the previous part, we created a node that publishes on a topic and makes your robot move, but it never stops. In this part, we will listen to the ```/odom``` topic and use that information to stop when the robot has moved more than 1.0 meters away from its start position.
Check in the Python or C++ tutorial to see how you can subscribe to a topic. You will have to somehow combine together the publisher and subscriber classes. We strongly advise that you follow an incremental approach:
* Add a subscriber to your ```lab2_node```. **You should have a single class**, so add the `subscribe` and `callback` to your existing class. This is different from the ROS tutorials where the `subscribe` and `callback` are in different classes. By default, the `spin` function is blocking and accepts a single `node` as an argument.
* Add a printout in the callback to check that you received a message.
* Modify your program so that it exits once the robot has moved by 1.0 meters.
Command Line Parameters
----------
For now, we have hard-coded parameters, such as the velocity and the distance. To make our node more effective, we should set those as parameters on the command line.
So that we can run our program this way:
```bash
ros2 run air_lab2 lab2_node --ros-args -p linear:=0.5 -p angular:=0.1 -p distance:=0.4
```
Before starting, you should study the [official Python tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-Python.html) or the [official C++ tutorial](https://docs.ros.org/en/galactic/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html).
Create a ```linear```, ```angular``` and ```distance``` parameter.
Navigation and Path Planning
----------------------------
The turtlebot4 includes a motion planner that can navigate and avoid obstacles. This is implemented as an action that can be started from the terminal with the following command:
```bash
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 6.0, y: 2.0, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"
```
It can also be started from RViz. Before we start, you should add the following visualizations (similarly to Lab1, two types should be used, i.e., ```Map``` and ```Path```, add two ``M̀ap```and two ```Path```):
* ```/global_costmap/costmap``` and change the colorscheme to costmap (```Map```)
* ```/local_costmap/costmap``` and change the colorscheme to costmap (```M̀ap```)
* ```/plan``` (```Path```)
* ```/local_plan``` (```Path```) and change the color to red
If you run the ```NavigateToPose``` action, you should see the following in ```Rviz```:
![rviz_path_planning](uploads/1276ecebbfd573d82fd7bd64ae3c076f/rviz_path_planning.png)
We can also send ```NavigateToPose``` requests directly from ```Rviz```. To do so we need to add the following:
* In ```Panels``` menu, select ```Add new panel``` and then ```Navigation 2```
* In the toolbar, click on the plus, then select ```GoalTool``` under ```nav2_rviz_plugin```
Now you can select ```Nav2 goal``` tool and click in Rviz to make your robot move.
![rviz2_navigation_2](uploads/6b7d4787d3ff24dfb3f94fed12b10bab/rviz2_navigation_2.png)
Exploration with a Random Pattern
---------------------------------
We will now create a new node that will send the robot to a random location to explore the map. We will need to create a new node in the ```air_lab2``` module.
### Python
Create a new file in ```$HOME/TDDE05/ros2_ws/src/labs/air_lab2/air_lab2``` called ```random_exploration.py```. This will be your new node. You need to modify your ```setup.py``` so that it contains the following change to ```entry_points```:
```python
entry_points={
'console_scripts': [
'lab2_node = air_lab2.lab2_node:main',
'random_exploration = air_lab2.random_exploration:main'
],
},
```
We will use the ```NavigateToPose``` action. You can check its type with:
```bash
ros2 interface show nav2_msgs/action/NavigateToPose
```
Before starting the implementation, study the [official action tutorial](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html).
We already modified the tutorial, so that you can use the following in ```random_exploration.py``` to go to a single location:
```python
import rclpy
import math
from rclpy.action import ActionClient
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
class RandomExploration(Node):
def __init__(self):
super().__init__('random_exploration')
self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
def send_goal(self, x, y, angle):
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = "map"
goal_msg.pose.pose.position.x = x
goal_msg.pose.pose.position.y = y
goal_msg.pose.pose.orientation.w = math.cos(angle/2)
goal_msg.pose.pose.orientation.z = math.sin(angle/2)
self._action_client.wait_for_server()
self._send_goal_future = self._action_client.send_goal_async(goal_msg, self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
self._goal_handle = future.result()
if not self._goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = self._goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
rclpy.shutdown()
def feedback_callback(self, feedback_msg):
print(feedback_msg.feedback)
self.get_logger().info('Received feedback: {0}'.format(feedback_msg.feedback.navigation_time))
if feedback_msg.feedback.navigation_time.sec > 60:
self._goal_handle.cancel_goal_async()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
action_client = RandomExploration()
action_client.send_goal(10.0, 5.0, 1.0)
rclpy.spin(action_client)
if __name__ == '__main__':
main()
```
Extend that class so that you keep going to a new random location, either when the robot has reached its current location or when it is failing to reach it. Instead of basing yourself on time, you should check using feedback if the robot is moving. There are at least two approaches to solving that problem. One is much easier to implement than the other, so look carefully at the message structure.
### C++
Create a new file in ```$HOME/TDDE05/ros2_ws/src/labs/air_lab2/src``` called ```random_exploration.cpp```. This will be your new node. You need to modify your ```CMakeLists.txt```, to duplicate what is available for ```lab2_node```.
We will use the ```NavigateToPose``` action. You can check its type with:
```bash
ros2 interface show nav2_msgs/action/NavigateToPose
```
Before starting the implementation, study the [official action tutorial](https://docs.ros.org/en/galactic/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html).
We already modified the tutorial, so that you can use the following in ```random_exploration.py``` to go to a single location:
```c++
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
class RandomExploration : public rclcpp::Node
{
public:
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
explicit RandomExploration()
: Node("random_exploration")
{
this->client_ptr_ = rclcpp_action::create_client<NavigateToPose>(
this,
"navigate_to_pose");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&RandomExploration::send_goal, this));
}
void send_goal()
{
double x = 10.0;
double y = 5.0;
double angle = 1.0;
auto goal_msg = NavigateToPose::Goal();
goal_msg.pose.header.frame_id = "map";
goal_msg.pose.pose.position.x = x;
goal_msg.pose.pose.position.y = y;
goal_msg.pose.pose.orientation.w = std::cos(angle/2);
goal_msg.pose.pose.orientation.z = std::sin(angle/2);
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&RandomExploration::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&RandomExploration::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&RandomExploration::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
std::thread cancel_thread_;
void goal_response_callback(std::shared_future<GoalHandleNavigateToPose::SharedPtr> future)
{
auto goal_handle = future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleNavigateToPose::SharedPtr goal_handle,
const std::shared_ptr<const NavigateToPose::Feedback> feedback)
{
if(feedback->navigation_time.sec>60)
{
//FIXME as of ROS Galactic, it looks like async_cancel_goal is not working properly
// current work around is to cancel all goals.
//client_ptr_->async_cancel_goal(goal_handle);
client_ptr_->async_cancel_all_goals();
RCLCPP_INFO(this->get_logger(), "Timeout");
rclcpp::shutdown();
}
}
void result_callback(const GoalHandleNavigateToPose::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Goal was succeeded");
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
rclcpp::shutdown();
}
}; // class RandomExploration
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<RandomExploration>());
rclcpp::shutdown();
return 0;
}
```
Extend that class so that you keep going to a new random location, either when the robot has reached its current location or when it is failing to reach it. Instead of basing yourself on time, you should check using feedback if the robot is moving. There are at least two approaches to solving that problem. One is much easier to implement than the other, so look carefully at the message structure.
Demonstration
--------------
* run your lab2_node and random_exploration nodes
* show your screen file and that the map is loaded correctly