Only released in EOL distros:
Package Summary
The goto_crossing package provides a goToGoal behavior to a crossing center if the crossing has at least three frontiers and some kind of move forward behavior if it has two or less frontiers.
- Maintainer status: developed
- Maintainer: Gaël Ecorchard <gael.ecorchard AT ciirc.cvut DOT cz>
- Author: Gaël Ecorchard <gael.ecorchard AT ciirc.cvut DOT cz>
- License: BSD
- Bug / feature tracker: https://github.com/lama-imr/lama_utilities/issues
- Source: git https://github.com/lama-imr/lama_utilities.git (branch: indigo-devel)
The goto_crossing provides a Go-To-Goal behavior to reach the center of a crossing (lama_msgs/Crossing message) for the Large Maps Framework (LaMa).
Contents
Overview
The goto_crossing provides a Go-To-Goal behavior to reach the center of a crossing (lama_msgs/Crossing message).
Can be used as a ROS node or as a class instance, as documented below.
Usage
As ROS Node
Launch the node with rosrun goto_crossing goto_crossing.
As Class Instance
To use the goto_crossing::CrossingGoer as class instance, one uses goto_crossing::CrossingGoer::goto_crossing to compute the twist necessary to reach the crossing center:
1 #include <geometry_msgs/Twist.h>
2 #include <goto_crossing/crossing_goer.h>
3 #include <lama_msgs/Crossing.h>
4
5 goto_crossing::CrossingGoer crossing_goer;
6
7 lama_msgs::Crossing crossing;
8 geometry_msgs::Twist twist;
9 crossing.center.x = 0.56;
10 crossing.center.y = 0.24;
11 const bool goal_reached = crossing_goer.goto_crossing(crossing, twist);
To reset the integral parts of the PI-controller, use
1 crossing_goer.resetIntegrals();
ROS API
Subscribed Topics
~<name>/crossing (lama_msgs/Crossing)- Crossing which center attribute is the goal relative to the robot.
Published Topics
~<name>/cmd_vel (geometry_msgs/Twist)- The set velocities to reach the goal
- True if the goal is within reach_distance
Services
~<name>/reset_integrals (std_srvs/Empty)- Reset the integrated sum of errors
Parameters
~<name>/kp_v (float)- Proportional gain for the linear velocity (s^-1)
- Proportional gain for the angular velocity (s^-1)
- Integral gain for the linear velocity (s^-2)
- Integral gain for the angular velocity (s^-2)
- Minimum linear set velocity (m.s^-1), used if the integral gain is 0
- Maximum linear set velocity (m.s^-1). If set to 0, the linear velocity is not throttled.
- Minimum angular set velocity (m.s^-1), used if the integral gain is 0
- Maximum angular set velocity (rad.s^-1). If set to 0, the angular velocity is not throttled.
- Goal is reached if closer than this (m)
- If the goal is behind the robot, force the robot to turn left if the goal angle is within [pi - dtheta_force_left_, pi], even if the angle difference would tell that we should turn right. This is to avoid instability in the case that the angle would oscillate around pi. (rad)
- If dtheta is greater than this, only turn, do not go forward (rad)
- Anti wind-up for sum_v (m.s)
- Anti wind-up for sum_w_ (rad.s)}