A turtlebot3 patrolling the simulated and real robot pen/polygon. Patrolling means constantly moving and avoiding obstacles.
Checkpoint 5 (ROS 2 Topics)
Running:
cd ~/ros2_ws/src
git clone https://github.com/ivogeorg/robot_patrol.git
git checkout checkpoint-5
colcon build --packages-select robot_patrol
ros2 launch turtlebot3_gazebo main_turtlebot3_lab.launch.xml
ros2 launch robot_patrol start_patrolling.launch.py
Controlling the log verbosity level:
#include <rclcpp/rclcpp.hpp>
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared
auto logger = rclcpp::get_logger("robot_patrol_node");
// Set the log level to DEBUG if (rcutils_logging_set_logger_level( logger.get_name(), RCUTILS_LOG_SEVERITY_DEBUG) != RCUTILS_RET_OK) { // Handle the error (e.g., print an error message or throw an exception) RCLCPP_ERROR(logger, "Failed to set logger level for robot_patrol_node."); } else { RCLCPP_INFO(logger, "Successfully set logger level for robot_patrol_node."); }
rclcpp::spin(node); rclcpp::shutdown(); return 0; }
Screenshots | Turtlebot 3 Lab | Live |
---|---|---|
![]() |
![]() |
|
![]() |
![]() |
|
![]() |
![]() |
find_direction_heuristic
, a heuristic-based safety-criterion algorithm for filtering candidate directions. Directions are sorted on the basis of how close the neiboring directions' directions form an inverse quadratic function, and upon equality, on the size of the range.find_direction_foreground_buffers
, a buffer-based robot-clearance algorithm for filtering candidate directions. Forward obstacles are identified against the background of the walls. Clear spans are identified in a 360-deg circular array and safety buffer angles are used to pad each clear span at each end. Clear spans are then filtered by their resulting clearance and the remaining ones are sorted by range size. This last algorithm was motivated primarily by the existence of traffic-sign base obstacles that are both below the LIDAR scan plane and are wider than the widest traffic sign place, creating a hazard of catching a robot wheel on a traffic-sign base.find_direction_arc_sums
, an angle arc-based heuristic-clearance algorithm, akin to (2) but simpler. Currently running. Arc ranges are summed and sorted by two sums, one arc and three arcs with the former in the middle. The sorting is by the 3-arc sum first and then by the middle arc. It works fine in the simulator, but still has problems identifying the traffic-sign obstacles in the lab, especially if it does not approach them head on. More robust to inf
values in the lab scanner. range_min
, it is able to recognize the situation and back up to farther than OBSTACLE_FWD_PROXIMITY
(0.35 m).Checkpoint 6 - Part 1 (ROS 2 Services)
source ~/simulation_ws/install/setup.bash
ros2 launch turtlebot3_gazebo main_turtlebot3_lab.launch.xml
/cmd_vel
for moving in circles
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.05, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.3}}"
cd ~/ros2_ws
source install/setup.bash
ros2 launch robot_patrol start_direction_service.launch.py
cd ~/ros2_ws
source install/setup.bash
ros2 launch robot_patrol start_test_service.launch.py
cd ~/ros2_ws
source install/setup.bash
ros2 launch robot_patrol main.launch.py
Implementation notes
For custom messages that are (to be) generated in the same package where they will be used in source code, the following need to be added to the CMakeLists.txt
file:
sensor_msgs
for sensor_msgs/LaserScan
):
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/GetDirection.srv"
DEPENDENCIES sensor_msgs
)
add_executable(direction_service_node src/direction_service.cpp)
ament_target_dependencies(direction_service_node rclcpp)
rosidl_get_typesupport_target(typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(direction_service_node ${typesupport_target})
parametrize_laser_scanner
, as this is the only sure criterion to tell the lab from the simulator.go_to_pose
action server
cd ~/ros2_ws
source install/setup.bash
ros2 launch robot_patrol start_gotopos_action.launch.py
ros2 action send_goal -f /go_to_pos robot_control/action/GoToPose "goal_pos:
x: 0.7
y: 0.3
theta: 90.0
"
Screenshots | Robot at origin | Robot at (0.7, 0.3) |
---|---|---|
![]() |
![]() |
theta
parameter is in the world frame and is in degrees in both the request and the feedbacks.theta
in the world frame.[laser_scan_subscriber_node]: angle_min = -3.141600
[laser_scan_subscriber_node]: angle_max = 3.141600
[laser_scan_subscriber_node]: angle_increment = 0.009534
[laser_scan_subscriber_node]: range_min = 0.120000
[laser_scan_subscriber_node]: range_max = 30.000000
[laser_scan_subscriber_node]: ranges.size() = 660
[laser_scan_subscriber_node]: ranges[0] = 1.610980
[laser_scan_subscriber_node]: ranges[164] = 1.600037
[laser_scan_subscriber_node]: ranges[329] = 0.226959
[laser_scan_subscriber_node]: ranges[493] = 0.470259
[laser_scan_subscriber_node]: ranges[659] = 1.572943
/**
This shows that the turtlebot3 laser scan is parameterized thus:
329
_________
||| ^ |||
||| fwd |||
493 |l r| 164
| |
|___b___|
0 (659)
These are the indices of the ranges array:
329 - forward (also approximately 359)
493 - left
0 - backward
164 - right
*/
yaw_ = yaw_from_quaternion(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);`
atan2(2.0f * (w * z + x * y), w * w + x * x - y * y - z * z);
In short, angle zero being "forward along the x-axis" doesn't mean that index zero of the ranges
vector coincides. It is actually diametrically opposite.
tutrlebot3_burger.urdf
, there is no rotation in the joints between base_footprint
and base_link
, nor between base_link
and scan_base
.empty_world.launch.py
, the robot appears in default pose with the forward direction along the x-axis. The LIDAR is therefore aligned the same way.
In the simulator, I positioned the robot in such a way that it was unambiguous which index was 0, and it was the exact opposite of what ros2 interface show sensor_msgs/msg/LaserScan
states (below). My index 0 is backward (not forward) along the x-axis.
Readings:
angle_min = -3.141600
angle_max = 3.141600
angle_increment = 0.009534
range_min = 0.120000
range_max = 30.000000
ranges.size() = 660
ranges[0] = 0.498451
ranges[164] = 1.056713
ranges[329] = 0.195327
ranges[494] = 0.789187
ranges[659] = 0.485331
ros2 interface show sensor_msgs/msg/LaserScan
reads:
std_msgs/Header header # timestamp in the header is the acquisition time of
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
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] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
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]. If your
# device does not provide intensities, please leave
# the array empty.
The actual TurtleBot3 lab.
The hardest spots for the robot, where it tens to either start oscillating or (if the oscillation is actually only in the code) get stuck, are:
min_range
) to an obstacle.Remediation measures:
Detecting problematic cases:
find_safest_direction
with no significant x, y
movement.inf
values in the range [FRONT_FROM, FRONT_TO]
.SOS
state that would relax the requirements and allow the robot to extricate itself when it detects that it is stuck. Some of the parameters to relax:
extended = false
parameter to find_safest_direction
. Watch for wraparound. May need to normalize at the edges.cmd_vel_msg_.linear.x = -0.05;
) with close 360-degree monitoring of obstacles. range_min
and therefore show inf
values! (advanced) Add buffer space around obstacles to avoid the wheels catching the bases of the traffic signs. See lab camera views above.
Multithreading protection.
laser_scan_data_
or last_laser_scan_data_
.find_direction_buffers
[35, 36]
.DROP
or RISE
.index_zero
to 36
.size
.index_zero
to (index_zero - 1 + size) % size
.obstacle_marker
is true
(or 1
).DROP
, an obstacle starts at 36
, so start marking with 1
, else 0
.obstacle_marker
at every discontinuity.size
.index_zero
to (index_zero - 1 + size) % size
.span_start_ix
and span_end_ix
. Put in a std::pair
?[size - 1, 0]
.ROBOT_WIDTH_ANGLE
and ROBOT_HALF_WIDTH_ANGLE
in radians (double
). Define for a robot that is half of SMALLER_DIMENSION
from an obstacle it wants to pass on the side and the obstacle is of radius half-robot width around the single detectable point on the axis. So, ROBOT_HALF_WIDTH_ANGLE
is:
obstacle robot
------- -------
| *..|||..* ||<-- b/n Klein starts is robot width (0.2)
----\-- | | |
\ --|--
\ |
\ |<------ smaller_dim (1.85)
\ |
\ |
\|
. Call it
obstacle_ctr_range`.ROBOT_WIDTH_ANGLE
is cos(smaller_dim / obstacle_ctr_range)
.
>>> angle_increment = 0.009534446522593498
>>> smaller_dim = 1.85
>>> robot_width = 0.2
>>> half_smaller_dim = smaller_dim / 2.0
>>> obstacle_ctr_range = math.sqrt(half_smaller_dim**2 + robot_width**2)
>>> robot_width_angle = math.acos (half_smaller_dim / obstacle_ctr_range)
>>> buffer_span = robot_width_angle / angle_increment
>>> buffer_span
22.333582665213278
parametrize_laser_scanner
.LEFT_BUFFER
and RIGHT_BUFFER
in number of indices (int
) from ROBOT_WIDTH_ANGLE
. LEFT_BUFFER = RIGHT_BUFFER = 22;
LEFT_BUFFER
and RIGHT_BUFFER
to the span (std::pair
) and calculate new width.left
and right
alloweable span, normal (+/- pi rad) or extended
(+/- 2 * pi rad).
extended = false
), intersect spans with left
-right
, and calculate new width.extended = true
), do not modify further.2 * LEFT_BUFFER
:
largest_range
, the largest range in the (buffered or buffer-reduced) index span.largest_range
and largest_range_index
to std::vector<std::pair<double, int>> clear_spans;
.clear_spans
by range in descending order.direction_
to the second element of the top/head entry of the sorted vector.stuck_threshold_
(x,y-space) ~0.10 CONSIDER!!!too_many_turns_threshold_
~5last_state_
takes care of all the cases where state logic depends on the previous state, specifically in State::STOPPED
when arriving from State::TURNING
and State::BACK_UP
.turns_
(after completing a turn)
State::TURNING
State::STOPPED
when starting forward linearis_oscillating_
State::STOPPED
What is the purpose? CONSIDER!!!too_close_to_obstacle_
State::STOPPED
enum class State { STOPPED, FORWARD, FIND_NEW_DIR, TURNING, BACK_UP };
std::tuple<bool, float>
from obstacle_in_range
is_oscillating_ = true;
, BACK_UP
just_backed_up_
=> set extended_range_ = true;
and FIND_NEW_DIR
extended_range_
, do extendedextended_range_ = false;
++turns_;
just_turned_ = true;
too_many_turns_threshold_
should be even, so as to back up in a direction that is less likely to be problematic just_backed_up_ = true;
=> STOPPEDfind_safest_direction
~
enum class DirSafetyBias { ANGLE, RANGE };
ANGLE
favors larger angles, RANGE
favors longer ranges. Bias applied after safety criterion!!!dir_safety_bias = DirSafetyBias::ANGLE
DirSafetyBias::RANGE
might work better with safety criterion, with DirSafetyBias::ANGLE
applied to help getting unstuck.enum class DirPref { RIGHT, LEFT, RIGHT_LEFT, NONE };
NONE
returns the index of the safest dir v_indexed_averages[0]
, LEFT_RIGHT
is based on the count of directions to the left and right.dir_bias = DirPref::NONE
just_turned_
and there is an obstacle (in STOPPED
)State::TURNING
calls to find_safest_direction
range_min
)~
inf
in obstacle_in_range
is_obstacle
and the ratio of inf
(to all)State::STOPPED
or State::FORWARD
CONSIDER!!!too_close_to_obstacle_
State::SOS
State::OSCILLATION
AND State::TOO_CLOSE
State::BACK_UP
is best, logic in State::STOPPED
ranges
array and identify abrupt drops and jumps in distance. Going CCW from 0
to RANGES_SIZE
, the first will be a drop and is the right edge, the second is going to be a jump and is the left edge, with the middle being the foreground obstacle.obstacle_in_range
or only in find_safest_direction
?_ turtlebot3_gazebo
.~
main...
.<visualize>
tag in the model.sdf
file for the burger
and/or waffle
bot, probably the former (which is the latest model).<visualize>
tag that made it into the SDF file, and change to true
. Changing the value in the model.sdf
file doesn't seem to be picked up.find_direction_midrange
F2B_RATIO_THRESHOLD
fails in certain cases.~
[laser_scan_subscriber_node-1] 265: 1.78352 (0)
[laser_scan_subscriber_node-1] 266: 1.76106 (0)
[laser_scan_subscriber_node-1] 267: 1.77472 (1) <-- caught (off-by-one`)
[laser_scan_subscriber_node-1] 268: 0.852214 (1) <-- caught
[laser_scan_subscriber_node-1] 269: 0.852501 (1)
...
[laser_scan_subscriber_node-1] 277: 0.896473 (1)
[laser_scan_subscriber_node-1] 278: 0.884809 (1) <-- missed
[laser_scan_subscriber_node-1] 279: 1.64608 (1) <-- missed
[laser_scan_subscriber_node-1] 280: 1.62386 (1)
direction_
to the top/head of the sorted vector.F2B_DIFF_THRESHOLD
.RISE
).~
[laser_scan_subscriber_node-1] 0: 0.354041 (0)
[laser_scan_subscriber_node-1] 1: 0.368825 (0)
...
[laser_scan_subscriber_node-1] 33: 0.901688 (0)
[laser_scan_subscriber_node-1] 34: 0.896493 (0)
[laser_scan_subscriber_node-1] 35: 0.903503 (RISE, 0)
[laser_scan_subscriber_node-1] 36: 1.87599 (0)
index_zero = 36
. index_zero
until (index_zero - 1 + size) % size
.
DROP
, it's the beginning of an obstacle, so start marking with 1
.RISE
, it's the end of an obstacle, so start marking with 0
. inf
.~
inf
does not compare, it is not caught by this algorithm.~F2B_DIFF_THRESHOLD
problem (see errata.md).~