HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
navigations/pose_follower/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,46 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package pose_follower
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.4.1 (2022-08-24)
------------------
0.4.0 (2022-03-07)
------------------
0.3.4 (2020-06-19)
------------------
* Initial release into noetic
* Set cmake_policy CMP0048 to fix warning
* Contributors: Martin Günther
0.3.3 (2019-10-15)
------------------
* Add dynamic reconfigure to pose_follower (`#40 <https://github.com/ros-planning/navigation_experimental/issues/40>`_)
Similar to the other available local planners, this commit adds dynamic reconfigure to pose_follower. In addition to this, the collision_planner parameters (used for detecting illegal trajectory) have been moved to the `PoseFollower/collision_planner` namespace.
Major ROS API changes:
* all internal TrajectoryPlannerROS parameters moved into the collision_planner namespace
* all internal TrajectoryPlannerROS publishers (cost_cloud, global_plan, local_plan) moved into the collision_planner namespace
* there is still a global_plan topic without namespace, which is now only published by the pose_follower itself and no longer shared with the internal TrajectoryPlannerROS.
* unused publisher removed (`#41 <https://github.com/ros-planning/navigation_experimental/issues/41>`_)
* Add READMEs
* Contributors: Martin Günther, Pavel Shumejko
0.3.2 (2019-01-16)
------------------
* max rotation vel in in-place rotation limited (`#27 <https://github.com/ros-planning/navigation_experimental/issues/27>`_)
* pose_follower: Add visualization of global plan (`#26 <https://github.com/ros-planning/navigation_experimental/issues/26>`_)
* Contributors: Martin Günther, Pavel, sumejko92
0.3.1 (2018-09-05)
------------------
0.3.0 (2018-09-04)
------------------
* Convert to TF2 + new navigation API (for melodic)
* Use non deprecated pluginlib macro + headers
* Contributors: Martin Günther
0.2.0 (2018-09-03)
------------------
* Initial release into indigo, kinetic, lunar and melodic

37
navigations/pose_follower/CMakeLists.txt Executable file
View File

@@ -0,0 +1,37 @@
cmake_minimum_required(VERSION 3.5.1)
cmake_policy(SET CMP0048 NEW)
project(pose_follower)
set(pose_follower_ROS_DEPS nav_core base_local_planner costmap_2d roscpp tf2_geometry_msgs tf2_ros nav_msgs pluginlib dynamic_reconfigure)
find_package(catkin REQUIRED COMPONENTS ${pose_follower_ROS_DEPS})
generate_dynamic_reconfigure_options(
cfg/PoseFollower.cfg
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES pose_follower
CATKIN_DEPENDS ${pose_follower_ROS_DEPS}
)
include_directories(include ${catkin_INCLUDE_DIRS})
add_library(pose_follower src/pose_follower.cpp)
add_dependencies(pose_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(pose_follower ${catkin_LIBRARIES})
install(TARGETS pose_follower
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
install(FILES blp_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

5
navigations/pose_follower/README.md Executable file
View File

@@ -0,0 +1,5 @@
pose_follower
=============
A implementation of a local planner that attempts to follow a plan as closely
as possible.

7
navigations/pose_follower/blp_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libpose_follower">
<class name="pose_follower/PoseFollower" type="pose_follower::PoseFollower" base_class_type="nav_core::BaseLocalPlanner">
<description>
A implementation of a local planner that attempts to follow a plan as closely as possible.
</description>
</class>
</library>

33
navigations/pose_follower/cfg/PoseFollower.cfg Executable file
View File

@@ -0,0 +1,33 @@
#!/usr/bin/env python
PACKAGE = 'pose_follower'
from math import pi
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t, str_t
gen = ParameterGenerator()
gen.add("max_vel_lin", double_t, 0, "Maximum linear velocity in m/s", 0.9, 0.0, 10.0)
gen.add("max_vel_th", double_t, 0, "Maximum angular velocity in rad/s", 1.4, 0.0, 10.0)
gen.add("min_vel_lin", double_t, 0, "Minimum linear velocity for the robot in m/s", 0.1, 0, 10.0)
gen.add("min_vel_th", double_t, 0, "Minimum angular velocity for the robot in rad/s", 0.0, 0, 10.0)
gen.add("min_in_place_vel_th", double_t, 0, "If we're rotating in place, go at least this fast to avoid getting stuck", 0.0, 0, 10.0)
gen.add("in_place_trans_vel", double_t, 0, "When we're near the end and would be trying to go no faster than this translationally, just rotate in place instead", 0.0, 0, 10.0)
gen.add("trans_stopped_velocity", double_t, 0, "The robot is stopped if the velocities are lower than this", 1e-4, 0, 10.0)
gen.add("rot_stopped_velocity", double_t, 0, "The robot is stopped if the velocities are lower than this", 1e-4, 0, 10.0)
gen.add("tolerance_trans", double_t, 0, "Translational tolerance for the goal", 0.02, 0, 10.0)
gen.add("tolerance_rot", double_t, 0, "Angular tolerance for the goal", 0.04, 0, pi)
gen.add("tolerance_timeout", double_t, 0, "We've reached our goal only if we're within range for this long and stopped", 0.5, 0, 20.0)
gen.add("samples", int_t, 0, "Number of samples (scaling factors of our current desired twist)", 10, 0, 20)
gen.add("allow_backwards", bool_t, 0, "Allow backwards movement", False)
gen.add("turn_in_place_first", bool_t, 0, "If true, turn in place to face the new goal instead of arching towards it", False)
gen.add("max_heading_diff_before_moving", double_t, 0, "If turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location", 0.17, 0, pi)
gen.add("k_trans", double_t, 0, "Gain factor for translation component of output velocities", 2.0, 0.0, 20.0)
gen.add("k_rot", double_t, 0, "Gain factor for rotation component of output velocities", 2.0, 0.0, 20.0)
exit(gen.generate(PACKAGE, "pose_follower", "PoseFollower"))

109
navigations/pose_follower/include/pose_follower/pose_follower.h Executable file
View File

@@ -0,0 +1,109 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef POSE_FOLLOWER_POSE_FOLLOWER_H_
#define POSE_FOLLOWER_POSE_FOLLOWER_H_
#include <ros/ros.h>
#include <tf2/convert.h>
#include <tf2/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <nav_core/base_local_planner.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <pose_follower/PoseFollowerConfig.h>
#include <dynamic_reconfigure/server.h>
#include <base_local_planner/trajectory_planner_ros.h>
namespace pose_follower {
class PoseFollower : public nav_core::BaseLocalPlanner {
public:
PoseFollower();
~PoseFollower();
void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
bool isGoalReached();
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
private:
inline double sign(double n){
return n < 0.0 ? -1.0 : 1.0;
}
geometry_msgs::Twist diff2D(const geometry_msgs::Pose& pose1, const geometry_msgs::Pose& pose2);
geometry_msgs::Twist limitTwist(const geometry_msgs::Twist& twist);
double headingDiff(double pt_x, double pt_y, double x, double y, double heading);
bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan);
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
bool stopped();
void publishPlan(const std::vector<geometry_msgs::PoseStamped> &path, const ros::Publisher &pub);
void reconfigureCB(pose_follower::PoseFollowerConfig &config, uint32_t level);
tf2_ros::Buffer *tf_;
costmap_2d::Costmap2DROS *costmap_ros_;
ros::Publisher global_plan_pub_;
boost::mutex odom_lock_;
ros::Subscriber odom_sub_;
nav_msgs::Odometry base_odom_;
ros::Time goal_reached_time_;
unsigned int current_waypoint_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
base_local_planner::TrajectoryPlannerROS collision_planner_;
dynamic_reconfigure::Server<pose_follower::PoseFollowerConfig> *dsrv_;
// Parameters
double max_vel_lin_, max_vel_th_;
double min_vel_lin_, min_vel_th_;
double min_in_place_vel_th_, in_place_trans_vel_;
double trans_stopped_velocity_, rot_stopped_velocity_;
double K_trans_, K_rot_, tolerance_trans_, tolerance_rot_;
double tolerance_timeout_;
int samples_;
bool allow_backwards_;
bool turn_in_place_first_;
double max_heading_diff_before_moving_;
bool holonomic_;
};
};
#endif

30
navigations/pose_follower/package.xml Executable file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>pose_follower</name>
<version>0.4.1</version>
<description>
A implementation of a local planner that attempts to follow a plan as closely as possible.
</description>
<maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
<author>Eitan Marder-Eppstein</author>
<license>BSD</license>
<url type="website">http://wiki.ros.org/pose_follower</url>
<url type="repository">https://github.com/ros-planning/navigation_experimental.git</url>
<url type="bugtracker">https://github.com/ros-planning/navigation_experimental/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>base_local_planner</depend>
<depend>costmap_2d</depend>
<depend>dynamic_reconfigure</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<export>
<nav_core plugin="${prefix}/blp_plugin.xml" />
</export>
</package>

416
navigations/pose_follower/src/pose_follower.cpp Executable file
View File

@@ -0,0 +1,416 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <nav_msgs/Path.h>
#include <pose_follower/pose_follower.h>
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(pose_follower::PoseFollower, nav_core::BaseLocalPlanner)
namespace pose_follower {
PoseFollower::PoseFollower(): tf_(NULL), costmap_ros_(NULL) {}
PoseFollower::~PoseFollower() {
if (dsrv_)
delete dsrv_;
}
void PoseFollower::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros){
tf_ = tf;
costmap_ros_ = costmap_ros;
current_waypoint_ = 0;
goal_reached_time_ = ros::Time::now();
ros::NodeHandle node_private("~/" + name);
collision_planner_.initialize(name + "/collision_planner", tf_, costmap_ros_);
//set this to true if you're using a holonomic robot
node_private.param("holonomic", holonomic_, true);
global_plan_pub_ = node_private.advertise<nav_msgs::Path>("global_plan", 1);
ros::NodeHandle node;
odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseFollower::odomCallback, this, _1));
dsrv_ = new dynamic_reconfigure::Server<pose_follower::PoseFollowerConfig>(
ros::NodeHandle(node_private));
dynamic_reconfigure::Server<pose_follower::PoseFollowerConfig>::CallbackType cb =
boost::bind(&PoseFollower::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
ROS_DEBUG("Initialized");
}
void PoseFollower::reconfigureCB(pose_follower::PoseFollowerConfig &config, uint32_t level) {
max_vel_lin_ = config.max_vel_lin;
max_vel_th_ = config.max_vel_th;
min_vel_lin_ = config.min_vel_lin;
min_vel_th_ = config.min_vel_th;
min_in_place_vel_th_ = config.min_in_place_vel_th;
in_place_trans_vel_ = config.in_place_trans_vel;
trans_stopped_velocity_ = config.trans_stopped_velocity;
rot_stopped_velocity_ = config.rot_stopped_velocity;
tolerance_trans_ = config.tolerance_trans;
tolerance_rot_ = config.tolerance_rot;
tolerance_timeout_ = config.tolerance_timeout;
samples_ = config.samples;
allow_backwards_ = config.allow_backwards;
turn_in_place_first_ = config.turn_in_place_first;
max_heading_diff_before_moving_ = config.max_heading_diff_before_moving;
K_trans_ = config.k_trans;
K_rot_ = config.k_rot;
}
void PoseFollower::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_lock_);
base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
}
double PoseFollower::headingDiff(double x, double y, double pt_x, double pt_y, double heading)
{
double v1_x = x - pt_x;
double v1_y = y - pt_y;
double v2_x = cos(heading);
double v2_y = sin(heading);
double perp_dot = v1_x * v2_y - v1_y * v2_x;
double dot = v1_x * v2_x + v1_y * v2_y;
//get the signed angle
double vector_angle = atan2(perp_dot, dot);
return -1.0 * vector_angle;
}
bool PoseFollower::stopped(){
//copy over the odometry information
nav_msgs::Odometry base_odom;
{
boost::mutex::scoped_lock lock(odom_lock_);
base_odom = base_odom_;
}
return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity_
&& fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity_
&& fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity_;
}
void PoseFollower::publishPlan(const std::vector<geometry_msgs::PoseStamped> &path,
const ros::Publisher &pub) {
// given an empty path we won't do anything
if (path.empty())
return;
// create a path message
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = path[0].header.frame_id;
gui_path.header.stamp = path[0].header.stamp;
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
for (unsigned int i = 0; i < path.size(); i++) {
gui_path.poses[i] = path[i];
}
pub.publish(gui_path);
}
bool PoseFollower::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
//get the current pose of the robot in the fixed frame
geometry_msgs::PoseStamped robot_pose;
if(!costmap_ros_->getRobotPose(robot_pose)){
ROS_ERROR("Can't get robot pose");
geometry_msgs::Twist empty_twist;
cmd_vel = empty_twist;
return false;
}
//we want to compute a velocity command based on our current waypoint
geometry_msgs::PoseStamped target_pose = global_plan_[current_waypoint_];
ROS_DEBUG("PoseFollower: current robot pose %f %f ==> %f", robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation));
ROS_DEBUG("PoseFollower: target robot pose %f %f ==> %f", target_pose.pose.position.x, target_pose.pose.position.y, tf2::getYaw(target_pose.pose.orientation));
//get the difference between the two poses
geometry_msgs::Twist diff = diff2D(target_pose.pose, robot_pose.pose);
ROS_DEBUG("PoseFollower: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
geometry_msgs::Twist limit_vel = limitTwist(diff);
geometry_msgs::Twist test_vel = limit_vel;
bool legal_traj = collision_planner_.checkTrajectory(test_vel.linear.x, test_vel.linear.y, test_vel.angular.z, true);
double scaling_factor = 1.0;
double ds = scaling_factor / samples_;
//let's make sure that the velocity command is legal... and if not, scale down
if(!legal_traj){
for(int i = 0; i < samples_; ++i){
test_vel.linear.x = limit_vel.linear.x * scaling_factor;
test_vel.linear.y = limit_vel.linear.y * scaling_factor;
test_vel.angular.z = limit_vel.angular.z * scaling_factor;
test_vel = limitTwist(test_vel);
if(collision_planner_.checkTrajectory(test_vel.linear.x, test_vel.linear.y, test_vel.angular.z, false)){
legal_traj = true;
break;
}
scaling_factor -= ds;
}
}
if(!legal_traj){
ROS_ERROR("Not legal (%.2f, %.2f, %.2f)", limit_vel.linear.x, limit_vel.linear.y, limit_vel.angular.z);
geometry_msgs::Twist empty_twist;
cmd_vel = empty_twist;
return false;
}
//if it is legal... we'll pass it on
cmd_vel = test_vel;
bool in_goal_position = false;
while(fabs(diff.linear.x) <= tolerance_trans_ &&
fabs(diff.linear.y) <= tolerance_trans_ &&
fabs(diff.angular.z) <= tolerance_rot_)
{
if(current_waypoint_ < global_plan_.size() - 1)
{
current_waypoint_++;
target_pose = global_plan_[current_waypoint_];
diff = diff2D(target_pose.pose, robot_pose.pose);
}
else
{
ROS_INFO("Reached goal: %d", current_waypoint_);
in_goal_position = true;
break;
}
}
//if we're not in the goal position, we need to update time
if(!in_goal_position)
goal_reached_time_ = ros::Time::now();
//check if we've reached our goal for long enough to succeed
if(goal_reached_time_ + ros::Duration(tolerance_timeout_) < ros::Time::now()){
geometry_msgs::Twist empty_twist;
cmd_vel = empty_twist;
}
return true;
}
bool PoseFollower::setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan){
current_waypoint_ = 0;
goal_reached_time_ = ros::Time::now();
if(!transformGlobalPlan(*tf_, global_plan, *costmap_ros_, costmap_ros_->getGlobalFrameID(), global_plan_)){
ROS_ERROR("Could not transform the global plan to the frame of the controller");
return false;
}
ROS_DEBUG("global plan size: %lu", global_plan_.size());
publishPlan(global_plan_, global_plan_pub_);
return true;
}
bool PoseFollower::isGoalReached(){
return goal_reached_time_ + ros::Duration(tolerance_timeout_) < ros::Time::now() && stopped();
}
geometry_msgs::Twist PoseFollower::diff2D(const geometry_msgs::Pose& pose1_msg,
const geometry_msgs::Pose& pose2_msg)
{
tf2::Transform pose1, pose2;
tf2::convert(pose1_msg, pose1);
tf2::convert(pose2_msg, pose2);
geometry_msgs::Twist res;
tf2::Transform diff = pose2.inverse() * pose1;
res.linear.x = diff.getOrigin().x();
res.linear.y = diff.getOrigin().y();
res.angular.z = tf2::getYaw(diff.getRotation());
if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
return res;
//in the case that we're not rotating to our goal position and we have a non-holonomic robot
//we'll need to command a rotational velocity that will help us reach our desired heading
//we want to compute a goal based on the heading difference between our pose and the target
double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
pose2.getOrigin().x(), pose2.getOrigin().y(), tf2::getYaw(pose2.getRotation()));
//we'll also check if we can move more effectively backwards
if (allow_backwards_)
{
double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf2::getYaw(pose2.getRotation()));
//check if its faster to just back up
if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
yaw_diff = neg_yaw_diff;
}
}
//compute the desired quaterion
tf2::Quaternion rot_diff;
rot_diff.setRPY(0.0, 0.0, yaw_diff);
tf2::Quaternion rot = pose2.getRotation() * rot_diff;
tf2::Transform new_pose = pose1;
new_pose.setRotation(rot);
diff = pose2.inverse() * new_pose;
res.linear.x = diff.getOrigin().x();
res.linear.y = diff.getOrigin().y();
res.angular.z = tf2::getYaw(diff.getRotation());
return res;
}
geometry_msgs::Twist PoseFollower::limitTwist(const geometry_msgs::Twist& twist)
{
geometry_msgs::Twist res = twist;
res.linear.x *= K_trans_;
if(!holonomic_)
res.linear.y = 0.0;
else
res.linear.y *= K_trans_;
res.angular.z *= K_rot_;
//if turn_in_place_first is true, see if we need to rotate in place to face our goal first
if (turn_in_place_first_ && fabs(twist.angular.z) > max_heading_diff_before_moving_)
{
res.linear.x = 0;
res.linear.y = 0;
if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
return res;
}
//make sure to bound things by our velocity limits
double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y);
if (lin_overshoot > 1.0)
{
res.linear.x /= lin_overshoot;
res.linear.y /= lin_overshoot;
}
//we only want to enforce a minimum velocity if we're not rotating in place
if(lin_undershoot > 1.0)
{
res.linear.x *= lin_undershoot;
res.linear.y *= lin_undershoot;
}
if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
if (std::isnan(res.linear.x))
res.linear.x = 0.0;
if (std::isnan(res.linear.y))
res.linear.y = 0.0;
//we want to check for whether or not we're desired to rotate in place
if(sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_){
if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
res.linear.x = 0.0;
res.linear.y = 0.0;
}
ROS_DEBUG("Angular command %f", res.angular.z);
return res;
}
bool PoseFollower::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try{
if (global_plan.empty())
{
ROS_ERROR("Recieved plan with zero length");
return false;
}
geometry_msgs::TransformStamped transform;
transform = tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id);
tf2::Stamped<tf2::Transform> tf_transform;
tf2::convert(transform, tf_transform);
tf2::Stamped<tf2::Transform> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
for(unsigned int i = 0; i < global_plan.size(); ++i){
const geometry_msgs::PoseStamped& pose = global_plan[i];
tf2::convert(pose, tf_pose);
tf_pose.setData(tf_transform * tf_pose);
tf_pose.stamp_ = tf_transform.stamp_;
tf_pose.frame_id_ = global_frame;
tf2::toMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
}
}
catch(tf2::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf2::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf2::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (!global_plan.empty())
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
};