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

103
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp Executable file
View File

@@ -0,0 +1,103 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* costmap_controller_execution.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "mbf_costmap_nav/costmap_controller_execution.h"
namespace mbf_costmap_nav
{
CostmapControllerExecution::CostmapControllerExecution(
const std::string &controller_name,
const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
const ros::Publisher &vel_pub,
const ros::Publisher &goal_pub,
const TFPtr &tf_listener_ptr,
const CostmapWrapper::Ptr &costmap_ptr,
const MoveBaseFlexConfig &config)
: AbstractControllerExecution(controller_name, controller_ptr, vel_pub, goal_pub,
tf_listener_ptr, toAbstract(config)),
costmap_ptr_(costmap_ptr)
{
ros::NodeHandle private_nh("~");
private_nh.param("controller_lock_costmap", lock_costmap_, true);
}
CostmapControllerExecution::~CostmapControllerExecution()
{
}
mbf_abstract_nav::MoveBaseFlexConfig CostmapControllerExecution::toAbstract(const MoveBaseFlexConfig &config)
{
// copy the controller-related abstract configuration common to all MBF-based navigation
mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
abstract_config.controller_frequency = config.controller_frequency;
abstract_config.controller_patience = config.controller_patience;
abstract_config.controller_max_retries = config.controller_max_retries;
abstract_config.oscillation_timeout = config.oscillation_timeout;
abstract_config.oscillation_distance = config.oscillation_distance;
return abstract_config;
}
uint32_t CostmapControllerExecution::computeVelocityCmd(
const geometry_msgs::PoseStamped &robot_pose,
const geometry_msgs::TwistStamped &robot_velocity,
geometry_msgs::TwistStamped &vel_cmd,
std::string &message)
{
// Lock the costmap while planning, but following issue #4, we allow to move the responsibility to the planner itself
if (lock_costmap_)
{
boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_ptr_->getCostmap()->getMutex()));
return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
}
return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
}
bool CostmapControllerExecution::safetyCheck()
{
// Check that the observation buffers for the costmap are current, we don't want to drive blind
if (!costmap_ptr_->isCurrent())
{
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
return false;
}
return true;
}
} /* namespace mbf_costmap_nav */

718
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp Executable file
View File

@@ -0,0 +1,718 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* costmap_navigation_server.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include <tf/tf.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseArray.h>
#include <mbf_msgs/MoveBaseAction.h>
#include <mbf_abstract_nav/MoveBaseFlexConfig.h>
#include <actionlib/client/simple_action_client.h>
#include <nav_core_wrapper/wrapper_global_planner.h>
#include <nav_core_wrapper/wrapper_local_planner.h>
#include <nav_core_wrapper/wrapper_recovery_behavior.h>
#include "mbf_costmap_nav/footprint_helper.h"
#include "mbf_costmap_nav/costmap_navigation_server.h"
namespace mbf_costmap_nav
{
CostmapNavigationServer::CostmapNavigationServer(const TFPtr &tf_listener_ptr) :
AbstractNavigationServer(tf_listener_ptr),
recovery_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapRecovery"),
nav_core_recovery_plugin_loader_("nav_core", "nav_core::RecoveryBehavior"),
controller_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapController"),
nav_core_controller_plugin_loader_("nav_core", "nav_core::BaseLocalPlanner"),
planner_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapPlanner"),
nav_core_planner_plugin_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
global_costmap_ptr_(new CostmapWrapper("global_costmap", tf_listener_ptr_)),
local_costmap_ptr_(new CostmapWrapper("local_costmap", tf_listener_ptr_)),
setup_reconfigure_(false)
{
// advertise services and current goal topic
check_point_cost_srv_ = private_nh_.advertiseService("check_point_cost",
&CostmapNavigationServer::callServiceCheckPointCost, this);
check_pose_cost_srv_ = private_nh_.advertiseService("check_pose_cost",
&CostmapNavigationServer::callServiceCheckPoseCost, this);
check_path_cost_srv_ = private_nh_.advertiseService("check_path_cost",
&CostmapNavigationServer::callServiceCheckPathCost, this);
clear_costmaps_srv_ = private_nh_.advertiseService("clear_costmaps",
&CostmapNavigationServer::callServiceClearCostmaps, this);
// dynamic reconfigure server for mbf_costmap_nav configuration; also include abstract server parameters
dsrv_costmap_ = boost::make_shared<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >(private_nh_);
dsrv_costmap_->setCallback(boost::bind(&CostmapNavigationServer::reconfigure, this, _1, _2));
// initialize all plugins
initializeServerComponents();
// start all action servers
startActionServers();
}
CostmapNavigationServer::~CostmapNavigationServer()
{
// remove every plugin before its classLoader goes out of scope.
controller_plugin_manager_.clearPlugins();
planner_plugin_manager_.clearPlugins();
recovery_plugin_manager_.clearPlugins();
action_server_recovery_ptr_.reset();
action_server_exe_path_ptr_.reset();
action_server_get_path_ptr_.reset();
action_server_move_base_ptr_.reset();
}
mbf_abstract_nav::AbstractPlannerExecution::Ptr CostmapNavigationServer::newPlannerExecution(
const std::string &plugin_name,
const mbf_abstract_core::AbstractPlanner::Ptr plugin_ptr)
{
return boost::make_shared<mbf_costmap_nav::CostmapPlannerExecution>(
plugin_name,
boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(plugin_ptr),
global_costmap_ptr_,
last_config_);
}
mbf_abstract_nav::AbstractControllerExecution::Ptr CostmapNavigationServer::newControllerExecution(
const std::string &plugin_name,
const mbf_abstract_core::AbstractController::Ptr plugin_ptr)
{
return boost::make_shared<mbf_costmap_nav::CostmapControllerExecution>(
plugin_name,
boost::static_pointer_cast<mbf_costmap_core::CostmapController>(plugin_ptr),
vel_pub_,
goal_pub_,
tf_listener_ptr_,
local_costmap_ptr_,
last_config_);
}
mbf_abstract_nav::AbstractRecoveryExecution::Ptr CostmapNavigationServer::newRecoveryExecution(
const std::string &plugin_name,
const mbf_abstract_core::AbstractRecovery::Ptr plugin_ptr)
{
return boost::make_shared<mbf_costmap_nav::CostmapRecoveryExecution>(
plugin_name,
boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(plugin_ptr),
tf_listener_ptr_,
global_costmap_ptr_,
local_costmap_ptr_,
last_config_);
}
mbf_abstract_core::AbstractPlanner::Ptr CostmapNavigationServer::loadPlannerPlugin(const std::string &planner_type)
{
mbf_abstract_core::AbstractPlanner::Ptr planner_ptr;
try
{
planner_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractPlanner>(
planner_plugin_loader_.createInstance(planner_type));
std::string planner_name = planner_plugin_loader_.getName(planner_type);
ROS_DEBUG_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded.");
}
catch (const pluginlib::PluginlibException &ex_mbf_core)
{
ROS_DEBUG_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin."
<< " Try to load as a nav_core-based plugin. " << ex_mbf_core.what());
try
{
// For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
boost::shared_ptr<nav_core::BaseGlobalPlanner> nav_core_planner_ptr = nav_core_planner_plugin_loader_.createInstance(planner_type);
planner_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperGlobalPlanner>(nav_core_planner_ptr);
std::string planner_name = nav_core_planner_plugin_loader_.getName(planner_type);
ROS_DEBUG_STREAM("nav_core-based planner plugin " << planner_name << " loaded");
}
catch (const pluginlib::PluginlibException &ex_nav_core)
{
ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it's properly registered"
<< " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
}
}
return planner_ptr;
}
bool CostmapNavigationServer::initializePlannerPlugin(
const std::string &name,
const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr
)
{
mbf_costmap_core::CostmapPlanner::Ptr costmap_planner_ptr
= boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(planner_ptr);
ROS_DEBUG_STREAM("Initialize planner \"" << name << "\".");
if (!global_costmap_ptr_)
{
ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
return false;
}
costmap_planner_ptr->initialize(name, global_costmap_ptr_.get());
ROS_DEBUG("Planner plugin initialized.");
return true;
}
mbf_abstract_core::AbstractController::Ptr CostmapNavigationServer::loadControllerPlugin(const std::string &controller_type)
{
mbf_abstract_core::AbstractController::Ptr controller_ptr;
try
{
controller_ptr = controller_plugin_loader_.createInstance(controller_type);
std::string controller_name = controller_plugin_loader_.getName(controller_type);
ROS_DEBUG_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded.");
}
catch (const pluginlib::PluginlibException &ex_mbf_core)
{
ROS_DEBUG_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;"
<< " we will retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
try
{
// For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
boost::shared_ptr<nav_core::BaseLocalPlanner> nav_core_controller_ptr
= nav_core_controller_plugin_loader_.createInstance(controller_type);
controller_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperLocalPlanner>(nav_core_controller_ptr);
std::string controller_name = nav_core_controller_plugin_loader_.getName(controller_type);
ROS_DEBUG_STREAM("nav_core-based controller plugin " << controller_name << " loaded.");
}
catch (const pluginlib::PluginlibException &ex_nav_core)
{
ROS_FATAL_STREAM("Failed to load the " << controller_type << " controller, are you sure it's properly registered"
<< " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
}
}
return controller_ptr;
}
bool CostmapNavigationServer::initializeControllerPlugin(
const std::string &name,
const mbf_abstract_core::AbstractController::Ptr &controller_ptr)
{
ROS_DEBUG_STREAM("Initialize controller \"" << name << "\".");
if (!tf_listener_ptr_)
{
ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
return false;
}
if (!local_costmap_ptr_)
{
ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
return false;
}
mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr
= boost::static_pointer_cast<mbf_costmap_core::CostmapController>(controller_ptr);
costmap_controller_ptr->initialize(name, tf_listener_ptr_.get(), local_costmap_ptr_.get());
ROS_DEBUG_STREAM("Controller plugin \"" << name << "\" initialized.");
return true;
}
mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPlugin(
const std::string &recovery_type)
{
mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr;
try
{
recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
recovery_plugin_loader_.createInstance(recovery_type));
std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
ROS_DEBUG_STREAM("mbf_costmap_core-based recovery behavior plugin " << recovery_name << " loaded.");
}
catch (pluginlib::PluginlibException &ex_mbf_core)
{
ROS_DEBUG_STREAM("Failed to load the " << recovery_type << " recovery behavior as a mbf_costmap_core-based plugin;"
<< " Retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
try
{
// For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
boost::shared_ptr<nav_core::RecoveryBehavior> nav_core_recovery_ptr =
nav_core_recovery_plugin_loader_.createInstance(recovery_type);
recovery_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperRecoveryBehavior>(nav_core_recovery_ptr);
std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
ROS_DEBUG_STREAM("nav_core-based recovery behavior plugin " << recovery_name << " loaded.");
}
catch (const pluginlib::PluginlibException &ex_nav_core)
{
ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
<< " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
}
}
return recovery_ptr;
}
bool CostmapNavigationServer::initializeRecoveryPlugin(
const std::string &name,
const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr)
{
ROS_DEBUG_STREAM("Initialize recovery behavior \"" << name << "\".");
if (!tf_listener_ptr_)
{
ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
return false;
}
if (!local_costmap_ptr_)
{
ROS_FATAL_STREAM("The local costmap pointer has not been initialized!");
return false;
}
if (!global_costmap_ptr_)
{
ROS_FATAL_STREAM("The global costmap pointer has not been initialized!");
return false;
}
mbf_costmap_core::CostmapRecovery::Ptr behavior =
boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(behavior_ptr);
behavior->initialize(name, tf_listener_ptr_.get(), global_costmap_ptr_.get(), local_costmap_ptr_.get());
ROS_DEBUG_STREAM("Recovery behavior plugin \"" << name << "\" initialized.");
return true;
}
void CostmapNavigationServer::stop()
{
AbstractNavigationServer::stop();
ROS_INFO_STREAM_NAMED("mbf_costmap_nav", "Stopping local and global costmap for shutdown");
local_costmap_ptr_->stop();
global_costmap_ptr_->stop();
}
void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level)
{
// Make sure we have the original configuration the first time we're called, so we can restore it if needed
if (!setup_reconfigure_)
{
default_config_ = config;
setup_reconfigure_ = true;
}
if (config.restore_defaults)
{
config = default_config_;
// if someone sets restore defaults on the parameter server, prevent looping
config.restore_defaults = false;
}
// fill the abstract configuration common to all MBF-based navigation
mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
abstract_config.planner_frequency = config.planner_frequency;
abstract_config.planner_patience = config.planner_patience;
abstract_config.planner_max_retries = config.planner_max_retries;
abstract_config.controller_frequency = config.controller_frequency;
abstract_config.controller_patience = config.controller_patience;
abstract_config.controller_max_retries = config.controller_max_retries;
abstract_config.recovery_enabled = config.recovery_enabled;
abstract_config.recovery_patience = config.recovery_patience;
abstract_config.oscillation_timeout = config.oscillation_timeout;
abstract_config.oscillation_distance = config.oscillation_distance;
abstract_config.restore_defaults = config.restore_defaults;
mbf_abstract_nav::AbstractNavigationServer::reconfigure(abstract_config, level);
// also reconfigure costmaps
local_costmap_ptr_->reconfigure(config.shutdown_costmaps, config.shutdown_costmaps_delay);
global_costmap_ptr_->reconfigure(config.shutdown_costmaps, config.shutdown_costmaps_delay);
last_config_ = config;
}
bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request,
mbf_msgs::CheckPoint::Response &response)
{
// selecting the requested costmap
CostmapWrapper::Ptr costmap;
std::string costmap_name;
switch (request.costmap)
{
case mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP:
costmap = local_costmap_ptr_;
costmap_name = "local costmap";
break;
case mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP:
costmap = global_costmap_ptr_;
costmap_name = "global costmap";
break;
default:
ROS_ERROR_STREAM("No valid costmap provided; options are "
<< mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP << ": local costmap, "
<< mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP << ": global costmap");
return false;
}
// get target point as x, y coordinates
std::string costmap_frame = costmap->getGlobalFrameID();
geometry_msgs::PointStamped point;
if (!mbf_utility::transformPoint(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.point, point))
{
ROS_ERROR_STREAM("Transform target point to " << costmap_name << " frame '" << costmap_frame << "' failed");
return false;
}
double x = point.point.x;
double y = point.point.y;
// ensure costmap is active so cost reflects latest sensor readings
costmap->checkActivate();
unsigned int mx, my;
if (!costmap->getCostmap()->worldToMap(x, y, mx, my))
{
// point is outside of the map
response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::OUTSIDE);
ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is outside the map (cost = " << response.cost << ")");
}
else
{
// lock costmap so content doesn't change while checking cell costs
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
// get cost of cell under point and classify as one of the states: UNKNOWN, LETHAL, INSCRIBED, FREE
response.cost = costmap->getCostmap()->getCost(mx, my);
switch (response.cost)
{
case costmap_2d::NO_INFORMATION:
response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::UNKNOWN);
ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in unknown space! (cost = " << response.cost << ")");
break;
case costmap_2d::LETHAL_OBSTACLE:
response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::LETHAL);
ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in collision! (cost = " << response.cost << ")");
break;
case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::INSCRIBED);
ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is near an obstacle (cost = " << response.cost << ")");
break;
default:
response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::FREE);
ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is free (cost = " << response.cost << ")");
break;
}
}
costmap->checkDeactivate();
return true;
}
bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request,
mbf_msgs::CheckPose::Response &response)
{
// selecting the requested costmap
CostmapWrapper::Ptr costmap;
std::string costmap_name;
switch (request.costmap)
{
case mbf_msgs::CheckPose::Request::LOCAL_COSTMAP:
costmap = local_costmap_ptr_;
costmap_name = "local costmap";
break;
case mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP:
costmap = global_costmap_ptr_;
costmap_name = "global costmap";
break;
default:
ROS_ERROR_STREAM("No valid costmap provided; options are "
<< mbf_msgs::CheckPose::Request::LOCAL_COSTMAP << ": local costmap, "
<< mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP << ": global costmap");
return false;
}
// get target pose or current robot pose as x, y, yaw coordinates
std::string costmap_frame = costmap->getGlobalFrameID();
geometry_msgs::PoseStamped pose;
if (request.current_pose)
{
if (!mbf_utility::getRobotPose(*tf_listener_ptr_, robot_frame_, costmap_frame, ros::Duration(0.5), pose))
{
ROS_ERROR_STREAM("Get robot pose on " << costmap_name << " frame '" << costmap_frame << "' failed");
return false;
}
}
else
{
if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.pose, pose))
{
ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
return false;
}
}
double x = pose.pose.position.x;
double y = pose.pose.position.y;
double yaw = tf::getYaw(pose.pose.orientation);
// ensure costmap is active so cost reflects latest sensor readings
costmap->checkActivate();
// pad raw footprint to the requested safety distance; note that we discard footprint_padding parameter effect
std::vector<geometry_msgs::Point> footprint = costmap->getUnpaddedRobotFootprint();
costmap_2d::padFootprint(footprint, request.safety_dist);
// use footprint helper to get all the cells totally or partially within footprint polygon
std::vector<Cell> footprint_cells =
FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true);
response.state = mbf_msgs::CheckPose::Response::FREE;
if (footprint_cells.empty())
{
// no cells within footprint polygon must mean that robot is completely outside of the map
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::OUTSIDE));
}
else
{
// lock costmap so content doesn't change while adding cell costs
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
// integrate the cost of all cells; state value precedence is UNKNOWN > LETHAL > INSCRIBED > FREE
for (int i = 0; i < footprint_cells.size(); ++i)
{
unsigned char cost = costmap->getCostmap()->getCost(footprint_cells[i].x, footprint_cells[i].y);
switch (cost)
{
case costmap_2d::NO_INFORMATION:
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
break;
case costmap_2d::LETHAL_OBSTACLE:
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::LETHAL));
response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
break;
case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::INSCRIBED));
response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
break;
default:
response.cost += cost;
break;
}
}
}
// Provide some details of the outcome
switch (response.state)
{
case mbf_msgs::CheckPose::Response::OUTSIDE:
ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is outside the map (cost = " << response.cost
<< "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPose::Response::UNKNOWN:
ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in unknown space! (cost = " << response.cost
<< "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPose::Response::LETHAL:
ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in collision! (cost = " << response.cost
<< "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPose::Response::INSCRIBED:
ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is near an obstacle (cost = " << response.cost
<< "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPose::Response::FREE:
ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is free (cost = " << response.cost
<< "; safety distance = " << request.safety_dist << ")");
break;
}
costmap->checkDeactivate();
return true;
}
bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request,
mbf_msgs::CheckPath::Response &response)
{
// selecting the requested costmap
CostmapWrapper::Ptr costmap;
std::string costmap_name;
switch (request.costmap)
{
case mbf_msgs::CheckPath::Request::LOCAL_COSTMAP:
costmap = local_costmap_ptr_;
costmap_name = "local costmap";
break;
case mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP:
costmap = global_costmap_ptr_;
costmap_name = "global costmap";
break;
default:
ROS_ERROR_STREAM("No valid costmap provided; options are "
<< mbf_msgs::CheckPath::Request::LOCAL_COSTMAP << ": local costmap, "
<< mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP << ": global costmap");
return false;
}
// ensure costmap is active so cost reflects latest sensor readings
costmap->checkActivate();
// get target pose or current robot pose as x, y, yaw coordinates
std::string costmap_frame = costmap->getGlobalFrameID();
std::vector<geometry_msgs::Point> footprint;
if (!request.path_cells_only)
{
// unless we want to check just the cells directly traversed by the path, pad raw footprint
// to the requested safety distance; note that we discard footprint_padding parameter effect
footprint = costmap->getUnpaddedRobotFootprint();
costmap_2d::padFootprint(footprint, request.safety_dist);
}
// lock costmap so content doesn't change while adding cell costs
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
geometry_msgs::PoseStamped pose;
response.state = mbf_msgs::CheckPath::Response::FREE;
for (int i = 0; i < request.path.poses.size(); ++i)
{
response.last_checked = i;
if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.path.poses[i], pose))
{
ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
return false;
}
double x = pose.pose.position.x;
double y = pose.pose.position.y;
double yaw = tf::getYaw(pose.pose.orientation);
std::vector<Cell> cells_to_check;
if (request.path_cells_only)
{
Cell cell;
if (costmap->getCostmap()->worldToMap(x, y, cell.x, cell.y))
{
cells_to_check.push_back(cell); // out of map if false; cells_to_check will be empty
}
}
else
{
// use footprint helper to get all the cells totally or partially within footprint polygon
cells_to_check = FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true);
}
if (cells_to_check.empty())
{
// if path_cells_only is true, this means that current path's pose is outside the map
// if not, no cells within footprint polygon means that robot is completely outside of the map
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::OUTSIDE));
}
else
{
// integrate the cost of all cells; state value precedence is UNKNOWN > LETHAL > INSCRIBED > FREE
// we apply the requested cost multipliers if different from zero (default value)
for (int j = 0; j < cells_to_check.size(); ++j)
{
unsigned char cost = costmap->getCostmap()->getCost(cells_to_check[j].x, cells_to_check[j].y);
switch (cost)
{
case costmap_2d::NO_INFORMATION:
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
break;
case costmap_2d::LETHAL_OBSTACLE:
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::LETHAL));
response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
break;
case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::INSCRIBED));
response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
break;
default:response.cost += cost;
break;
}
}
}
if (request.return_on && response.state >= request.return_on)
{
// i-th pose state is bad enough for the client, so provide some details of the outcome and abort checking
switch (response.state)
{
case mbf_msgs::CheckPath::Response::OUTSIDE:
ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes outside the map "
<< "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPath::Response::UNKNOWN:
ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in unknown space! "
<< "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPath::Response::LETHAL:
ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in collision! "
<< "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPath::Response::INSCRIBED:
ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes near an obstacle "
<< "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
break;
case mbf_msgs::CheckPath::Response::FREE:
ROS_DEBUG_STREAM("Path is entirely free (maximum cost = "
<< response.cost << "; safety distance = " << request.safety_dist << ")");
break;
}
break;
}
i += request.skip_poses; // skip some poses to speedup processing (disabled by default)
}
costmap->checkDeactivate();
return true;
}
bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request &request,
std_srvs::Empty::Response &response)
{
// clear both costmaps
local_costmap_ptr_->clear();
global_costmap_ptr_->clear();
return true;
}
} /* namespace mbf_costmap_nav */

88
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_planner_execution.cpp Executable file
View File

@@ -0,0 +1,88 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* costmap_planner_execution.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include <nav_core_wrapper/wrapper_global_planner.h>
#include "mbf_costmap_nav/costmap_planner_execution.h"
namespace mbf_costmap_nav
{
CostmapPlannerExecution::CostmapPlannerExecution(
const std::string &planner_name,
const mbf_costmap_core::CostmapPlanner::Ptr &planner_ptr,
const CostmapWrapper::Ptr &costmap_ptr,
const MoveBaseFlexConfig &config)
: AbstractPlannerExecution(planner_name, planner_ptr, toAbstract(config)),
costmap_ptr_(costmap_ptr)
{
ros::NodeHandle private_nh("~");
private_nh.param("planner_lock_costmap", lock_costmap_, true);
}
CostmapPlannerExecution::~CostmapPlannerExecution()
{
}
mbf_abstract_nav::MoveBaseFlexConfig CostmapPlannerExecution::toAbstract(const MoveBaseFlexConfig &config)
{
// copy the planner-related abstract configuration common to all MBF-based navigation
mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
abstract_config.planner_frequency = config.planner_frequency;
abstract_config.planner_patience = config.planner_patience;
abstract_config.planner_max_retries = config.planner_max_retries;
return abstract_config;
}
uint32_t CostmapPlannerExecution::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
double dist_tolerance, double angle_tolerance,
std::vector<geometry_msgs::PoseStamped> &plan,
double &cost,
std::string &message)
{
if (lock_costmap_)
{
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap_ptr_->getCostmap()->getMutex()));
return planner_->makePlan(start, goal, dist_tolerance, angle_tolerance, plan, cost, message);
}
return planner_->makePlan(start, goal, dist_tolerance, angle_tolerance, plan, cost, message);
}
} /* namespace mbf_costmap_nav */

72
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_recovery_execution.cpp Executable file
View File

@@ -0,0 +1,72 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* costmap_recovery_execution.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include <nav_core/recovery_behavior.h>
#include "nav_core_wrapper/wrapper_recovery_behavior.h"
#include "mbf_costmap_nav/costmap_recovery_execution.h"
namespace mbf_costmap_nav
{
CostmapRecoveryExecution::CostmapRecoveryExecution(
const std::string &recovery_name,
const mbf_costmap_core::CostmapRecovery::Ptr &recovery_ptr,
const TFPtr &tf_listener_ptr,
const CostmapWrapper::Ptr &global_costmap,
const CostmapWrapper::Ptr &local_costmap,
const MoveBaseFlexConfig &config)
: AbstractRecoveryExecution(recovery_name, recovery_ptr, tf_listener_ptr, toAbstract(config)),
global_costmap_(global_costmap), local_costmap_(local_costmap)
{
}
CostmapRecoveryExecution::~CostmapRecoveryExecution()
{
}
mbf_abstract_nav::MoveBaseFlexConfig CostmapRecoveryExecution::toAbstract(const MoveBaseFlexConfig &config)
{
// copy the recovery-related abstract configuration common to all MBF-based navigation
mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
abstract_config.recovery_enabled = config.recovery_enabled;
abstract_config.recovery_patience = config.recovery_patience;
return abstract_config;
}
} /* namespace mbf_costmap_nav */

139
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp Executable file
View File

@@ -0,0 +1,139 @@
/*
* Copyright 2019, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* costmap_wrapper.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "mbf_costmap_nav/costmap_wrapper.h"
namespace mbf_costmap_nav
{
CostmapWrapper::CostmapWrapper(const std::string &name, const TFPtr &tf_listener_ptr) :
costmap_2d::Costmap2DROS(name, *tf_listener_ptr),
shutdown_costmap_(false), costmap_users_(0), private_nh_("~")
{
// even if shutdown_costmaps is a dynamically reconfigurable parameter, we
// need it here to decide whether to start or not the costmap on starting up
private_nh_.param("shutdown_costmaps", shutdown_costmap_, false);
private_nh_.param("clear_on_shutdown", clear_on_shutdown_, false);
if (shutdown_costmap_)
// initialize costmap stopped if shutdown_costmaps parameter is true
stop();
else
// otherwise costmap_users_ is at least 1, as costmap is always active
++costmap_users_;
}
CostmapWrapper::~CostmapWrapper()
{
shutdown_costmap_timer_.stop();
}
void CostmapWrapper::reconfigure(double shutdown_costmap, double shutdown_costmap_delay)
{
shutdown_costmap_delay_ = ros::Duration(shutdown_costmap_delay);
if (shutdown_costmap_delay_.isZero())
ROS_WARN("Zero shutdown costmaps delay is not recommended, as it forces us to enable costmaps on each action");
if (shutdown_costmap_ && !shutdown_costmap)
{
checkActivate();
shutdown_costmap_ = shutdown_costmap;
}
if (!shutdown_costmap_ && shutdown_costmap)
{
shutdown_costmap_ = shutdown_costmap;
checkDeactivate();
}
}
void CostmapWrapper::clear()
{
// lock and clear costmap
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*getCostmap()->getMutex());
resetLayers();
}
void CostmapWrapper::checkActivate()
{
boost::mutex::scoped_lock sl(check_costmap_mutex_);
shutdown_costmap_timer_.stop();
// Activate costmap if we shutdown them when not moving and they are not already active. This method must be
// synchronized because start costmap can take up to 1/update freq., and concurrent calls to it can lead to segfaults
if (shutdown_costmap_ && !costmap_users_)
{
start();
ROS_DEBUG_STREAM("" << name_ << " activated");
}
++costmap_users_;
}
void CostmapWrapper::checkDeactivate()
{
boost::mutex::scoped_lock sl(check_costmap_mutex_);
--costmap_users_;
ROS_ASSERT_MSG(costmap_users_ >= 0, "Negative number (%d) of active users count!", costmap_users_);
if (shutdown_costmap_ && !costmap_users_)
{
// Delay costmap shutdown by shutdown_costmap_delay so we don't need to enable at each step of a normal
// navigation sequence, what is terribly inefficient; the timer is stopped on costmap re-activation and
// reset after every new call to deactivate
shutdown_costmap_timer_ =
private_nh_.createTimer(shutdown_costmap_delay_, &CostmapWrapper::deactivate, this, true);
}
}
void CostmapWrapper::deactivate(const ros::TimerEvent &event)
{
boost::mutex::scoped_lock sl(check_costmap_mutex_);
ROS_ASSERT_MSG(!costmap_users_, "Deactivating costmap with %d active users!", costmap_users_);
if (clear_on_shutdown_)
clear(); // do before stop, as some layers (e.g. obstacle and voxel) reactivate their subscribers on reset
stop();
ROS_DEBUG_STREAM("" << name_ << " deactivated");
}
} /* namespace mbf_costmap_nav */

237
navigations/move_base_flex/mbf_costmap_nav/src/mbf_costmap_nav/footprint_helper.cpp Executable file
View File

@@ -0,0 +1,237 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 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: TKruse
*********************************************************************/
#include "mbf_costmap_nav/footprint_helper.h"
namespace mbf_costmap_nav
{
void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector<Cell>& pts) {
//Bresenham Ray-Tracing
int deltax = abs(x1 - x0); // The difference between the x's
int deltay = abs(y1 - y0); // The difference between the y's
int x = x0; // Start x off at the first pixel
int y = y0; // Start y off at the first pixel
int xinc1, xinc2, yinc1, yinc2;
int den, num, numadd, numpixels;
Cell pt;
if (x1 >= x0) // The x-values are increasing
{
xinc1 = 1;
xinc2 = 1;
}
else // The x-values are decreasing
{
xinc1 = -1;
xinc2 = -1;
}
if (y1 >= y0) // The y-values are increasing
{
yinc1 = 1;
yinc2 = 1;
}
else // The y-values are decreasing
{
yinc1 = -1;
yinc2 = -1;
}
if (deltax >= deltay) // There is at least one x-value for every y-value
{
xinc1 = 0; // Don't change the x when numerator >= denominator
yinc2 = 0; // Don't change the y for every iteration
den = deltax;
num = deltax / 2;
numadd = deltay;
numpixels = deltax; // There are more x-values than y-values
}
else // There is at least one y-value for every x-value
{
xinc2 = 0; // Don't change the x for every iteration
yinc1 = 0; // Don't change the y when numerator >= denominator
den = deltay;
num = deltay / 2;
numadd = deltax;
numpixels = deltay; // There are more y-values than x-values
}
for (int curpixel = 0; curpixel <= numpixels; curpixel++)
{
pt.x = x; //Draw the current pixel
pt.y = y;
pts.push_back(pt);
num += numadd; // Increase the numerator by the top of the fraction
if (num >= den) // Check if numerator >= denominator
{
num -= den; // Calculate the new numerator value
x += xinc1; // Change the x as appropriate
y += yinc1; // Change the y as appropriate
}
x += xinc2; // Change the x as appropriate
y += yinc2; // Change the y as appropriate
}
}
void FootprintHelper::getFillCells(std::vector<Cell>& footprint){
//quick bubble sort to sort pts by x
Cell swap, pt;
unsigned int i = 0;
while (i < footprint.size() - 1) {
if (footprint[i].x > footprint[i + 1].x) {
swap = footprint[i];
footprint[i] = footprint[i + 1];
footprint[i + 1] = swap;
if(i > 0) {
--i;
}
} else {
++i;
}
}
i = 0;
Cell min_pt;
Cell max_pt;
unsigned int min_x = footprint[0].x;
unsigned int max_x = footprint[footprint.size() -1].x;
//walk through each column and mark cells inside the footprint
for (unsigned int x = min_x; x <= max_x; ++x) {
if (i >= footprint.size() - 1) {
break;
}
if (footprint[i].y < footprint[i + 1].y) {
min_pt = footprint[i];
max_pt = footprint[i + 1];
} else {
min_pt = footprint[i + 1];
max_pt = footprint[i];
}
i += 2;
while (i < footprint.size() && footprint[i].x == x) {
if(footprint[i].y < min_pt.y) {
min_pt = footprint[i];
} else if(footprint[i].y > max_pt.y) {
max_pt = footprint[i];
}
++i;
}
//loop though cells in the column
for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
pt.x = x;
pt.y = y;
footprint.push_back(pt);
}
}
}
/**
* get the cells of a footprint at a given position
*/
std::vector<Cell> FootprintHelper::getFootprintCells(
double x, double y, double theta,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D& costmap,
bool fill){
std::vector<Cell> footprint_cells;
//if we have no footprint... do nothing
if (footprint_spec.size() <= 1) {
unsigned int mx, my;
if (costmap.worldToMap(x, y, mx, my)) {
Cell center;
center.x = mx;
center.y = my;
footprint_cells.push_back(center);
}
return footprint_cells;
}
//pre-compute cos and sin values
double cos_th = cos(theta);
double sin_th = sin(theta);
double new_x, new_y;
unsigned int x0, y0, x1, y1;
unsigned int last_index = footprint_spec.size() - 1;
for (unsigned int i = 0; i < last_index; ++i) {
//find the cell coordinates of the first segment point
new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
//find the cell coordinates of the second segment point
new_x = x + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
new_y = y + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
}
//we need to close the loop, so we also have to raytrace from the last pt to first pt
new_x = x + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
new_y = y + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
new_x = x + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
new_y = y + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
if(fill) {
getFillCells(footprint_cells);
}
return footprint_cells;
}
} /* namespace mbf_costmap_nav */

83
navigations/move_base_flex/mbf_costmap_nav/src/move_base_server_node.cpp Executable file
View File

@@ -0,0 +1,83 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* move_base_server_node.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "mbf_costmap_nav/costmap_navigation_server.h"
#include <signal.h>
#include <mbf_utility/types.h>
#include <tf2_ros/transform_listener.h>
typedef boost::shared_ptr<mbf_costmap_nav::CostmapNavigationServer> CostmapNavigationServerPtr;
mbf_costmap_nav::CostmapNavigationServer::Ptr costmap_nav_srv_ptr;
void sigintHandler(int sig)
{
ROS_INFO_STREAM("Shutdown costmap navigation server.");
if(costmap_nav_srv_ptr)
{
costmap_nav_srv_ptr->stop();
}
ros::shutdown();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mbf_2d_nav_server", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
double cache_time;
private_nh.param("tf_cache_time", cache_time, 10.0);
signal(SIGINT, sigintHandler);
#ifdef USE_OLD_TF
TFPtr tf_listener_ptr(new TF(nh, ros::Duration(cache_time), true));
#else
TFPtr tf_listener_ptr(new TF(ros::Duration(cache_time)));
tf2_ros::TransformListener tf_listener(*tf_listener_ptr);
#endif
costmap_nav_srv_ptr = boost::make_shared<mbf_costmap_nav::CostmapNavigationServer>(tf_listener_ptr);
ros::spin();
// explicitly call destructor here, otherwise costmap_nav_srv_ptr will be
// destructed after tearing down internally allocated static variables
costmap_nav_srv_ptr.reset();
return EXIT_SUCCESS;
}

82
navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_global_planner.cpp Executable file
View File

@@ -0,0 +1,82 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* wrapper_global_planner.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "nav_core_wrapper/wrapper_global_planner.h"
namespace mbf_nav_core_wrapper
{
uint32_t WrapperGlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
double dist_tolerance, double angle_tolerance,
std::vector<geometry_msgs::PoseStamped> &plan,
double &cost,
std::string &message)
{
#if ROS_VERSION_MINIMUM(1, 12, 0) // if current ros version is >= 1.12.0
// Kinetic and beyond
bool success = nav_core_plugin_->makePlan(start, goal, plan, cost);
#else
// Indigo
bool success = nav_core_plugin_->makePlan(start, goal, plan);
cost = 0;
#endif
message = success ? "Plan found" : "Planner failed";
return success ? 0 : 50; // SUCCESS | FAILURE
}
bool WrapperGlobalPlanner::cancel()
{
return false;
}
void WrapperGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
nav_core_plugin_->initialize(name, costmap_ros);
}
WrapperGlobalPlanner::WrapperGlobalPlanner(boost::shared_ptr<nav_core::BaseGlobalPlanner> plugin)
: nav_core_plugin_(plugin)
{}
WrapperGlobalPlanner::~WrapperGlobalPlanner()
{}
}; /* namespace mbf_abstract_core */

94
navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_local_planner.cpp Executable file
View File

@@ -0,0 +1,94 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* wrapper_local_planner.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "nav_core_wrapper/wrapper_local_planner.h"
namespace mbf_nav_core_wrapper
{
uint32_t WrapperLocalPlanner::computeVelocityCommands(
const geometry_msgs::PoseStamped &robot_pose,
const geometry_msgs::TwistStamped &robot_velocity,
geometry_msgs::TwistStamped &cmd_vel,
std::string &message)
{
bool success = nav_core_plugin_->computeVelocityCommands(cmd_vel.twist);
message = success ? "Goal reached" : "Controller failed";
return success ? 0 : 100; // SUCCESS | FAILURE
}
bool WrapperLocalPlanner::isGoalReached()
{
return nav_core_plugin_->isGoalReached();
}
bool WrapperLocalPlanner::isGoalReached(double xy_tolerance, double yaw_tolerance)
{
return isGoalReached();
}
bool WrapperLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)
{
return nav_core_plugin_->setPlan(plan);
}
bool WrapperLocalPlanner::cancel()
{
ROS_WARN_STREAM("The cancel method is not implemented. "
"Note: you are running a nav_core based plugin, "
"which is wrapped into the MBF interface.");
return false;
}
void WrapperLocalPlanner::initialize(std::string name,
TF *tf,
costmap_2d::Costmap2DROS *costmap_ros)
{
nav_core_plugin_->initialize(name, tf, costmap_ros);
}
WrapperLocalPlanner::WrapperLocalPlanner(boost::shared_ptr<nav_core::BaseLocalPlanner> plugin)
: nav_core_plugin_(plugin)
{}
WrapperLocalPlanner::~WrapperLocalPlanner()
{}
}; /* namespace mbf_abstract_core */

72
navigations/move_base_flex/mbf_costmap_nav/src/nav_core_wrapper/wrapper_recovery_behavior.cpp Executable file
View File

@@ -0,0 +1,72 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. Neither the name of the copyright holder 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 HOLDER 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.
*
* wrapper_recovery_behavior.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include <mbf_msgs/RecoveryResult.h>
#include "nav_core_wrapper/wrapper_recovery_behavior.h"
namespace mbf_nav_core_wrapper
{
void WrapperRecoveryBehavior::initialize(std::string name, TF *tf,
costmap_2d::Costmap2DROS *global_costmap,
costmap_2d::Costmap2DROS *local_costmap)
{
nav_core_plugin_->initialize(name, tf, global_costmap, local_costmap);
}
uint32_t WrapperRecoveryBehavior::runBehavior(std::string &message)
{
nav_core_plugin_->runBehavior();
// TODO return a code for old API
return mbf_msgs::RecoveryResult::SUCCESS;
}
bool WrapperRecoveryBehavior::cancel()
{
return false;
}
WrapperRecoveryBehavior::WrapperRecoveryBehavior(boost::shared_ptr<nav_core::RecoveryBehavior> plugin)
: nav_core_plugin_(plugin)
{}
WrapperRecoveryBehavior::~WrapperRecoveryBehavior()
{}
}; /* namespace mbf_abstract_core */