Commit 2036e077 authored by CANNERE Xavier's avatar CANNERE Xavier
Browse files

Topic 4.1

parent cbafb8a9
......@@ -12,4 +12,5 @@ Welcome to my ROS2 Tutorial!
./topic1/t1.rst
./topic2/t2.rst
./topic3/t3.rst
./topic4/t4.rst
Introduction to ROS2
====================
Step 1 : Setup
--------------
First, we will install ISAE package::
mkdir -p ~/colcon_ws/src
cd ~/colcon_ws/src
git clone https://gitlab.isae-supaero.fr/p.chauvin/awesome_ros2_packages.git
Then, build your ROS workspace::
cd ..
colcon build
And source your workspace::
. /install/local_setup.bash
Step 2 : Import Ros Node
------------------------
Open QtCreator and import your workspace:
.. figure:: 1.png
:width: 400
:align: center
:alt: image non disponible
Then fill that:
.. figure:: 2.png
:width: 400
:align: center
:alt: image non disponible
*Make sure it is the same things*
Now you are supposed to have this:
.. figure:: 3.png
:width: 400
:align: center
:alt: image non disponible
We'll now create our package and his node, type on a terminal::
cd ~/colcon_ws/src
ros2 pkg create --build-type ament_cmake --node-name follower pkg_follower
Step 3 : The code of the node
-----------------------------
In the ``follower.cpp`` replace the existant code by::
// Copyright 2018 Louise Poubel.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <memory>
#include <utility>
class Follow : public rclcpp::Node
{
public:
/// \brief Follow node, which subscribes to laser scan messages and publishes
/// velocity commands.
Follow()
: Node("follow")
{
// Subscribe to sensor messages
auto sensor_qos = rclcpp::QoS(rclcpp::SensorDataQoS());
laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"laser_scan", sensor_qos,
std::bind(&Follow::OnSensorMsg, this, std::placeholders::_1));
// Advertise velocity commands
auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", default_qos);
}
private:
/// \brief Callback for sensor message subscriber
/// \param[in] _msg Laser scan message
void OnSensorMsg(const sensor_msgs::msg::LaserScan::SharedPtr _msg)
{
// Find closest hit
float min_range = _msg->range_max + 1;
int idx = -1;
for (auto i = 0u; i < _msg->ranges.size(); ++i) {
auto range = _msg->ranges[i];
if (range > _msg->range_min && range < _msg->range_max && range < min_range) {
min_range = range;
idx = i;
}
}
// Calculate desired yaw change
double turn = _msg->angle_min + _msg->angle_increment * idx;
// Populate command message, all weights have been calculated by trial and error
auto cmd_msg = std::make_unique<geometry_msgs::msg::Twist>();
// Bad readings, stop
if (idx < 0) {
cmd_msg->linear.x = 0;
cmd_msg->angular.z = 0;
} else if (min_range <= min_dist_) {
// Too close, just rotate
cmd_msg->linear.x = 0;
cmd_msg->angular.z = turn * angular_k_;
} else {
cmd_msg->linear.x = linear_k_ / abs(turn);
cmd_msg->angular.z = turn * angular_k_;
}
cmd_pub_->publish(std::move(cmd_msg));
}
/// \brief Laser messages subscriber
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
/// \brief Velocity command publisher
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
/// \brief Minimum allowed distance from target
double min_dist_ = 1.0;
/// \brief Scale linear velocity, chosen by trial and error
double linear_k_ = 0.02;
/// \brief Scale angular velocity, chosen by trial and error
double angular_k_ = 0.08;
};
int main(int argc, char * argv[])
{
// Forward command line arguments to ROS
rclcpp::init(argc, argv);
// Create a node
auto node = std::make_shared<Follow>();
// Run node until it's exited
rclcpp::spin(node);
// Clean up
rclcpp::shutdown();
return 0;
}
Then build it, there is many errors don't worry, replace the content of ``package.xml`` by that::
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dolly_follow</name>
<version>0.4.0</version>
<description>
Follow node for Dolly, the robot sheep.
</description>
<maintainer email="burajiru.no.chapulina@gmail.com">Louise Poubel</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>rclcpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
And CMakeLists.txt by that::
cmake_minimum_required(VERSION 3.5)
project(dolly_follow)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
set(target "follower")
add_executable(${target} src/${target}.cpp)
ament_target_dependencies(${target}
"rclcpp"
"geometry_msgs"
"sensor_msgs")
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
install(TARGETS ${target}
DESTINATION lib/${PROJECT_NAME})
ament_package()
Now you can ``colcon build`` in the root of your workspace and your follower node compile correctly.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment