git commit -m "first commit"
This commit is contained in:
@@ -0,0 +1,475 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* abstract_controller_execution.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <mbf_msgs/ExePathResult.h>
|
||||
|
||||
#include "mbf_abstract_nav/abstract_controller_execution.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
const double AbstractControllerExecution::DEFAULT_CONTROLLER_FREQUENCY = 100.0; // 100 Hz
|
||||
|
||||
AbstractControllerExecution::AbstractControllerExecution(
|
||||
const std::string &name,
|
||||
const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
|
||||
const ros::Publisher &vel_pub,
|
||||
const ros::Publisher &goal_pub,
|
||||
const TFPtr &tf_listener_ptr,
|
||||
const MoveBaseFlexConfig &config) :
|
||||
AbstractExecutionBase(name),
|
||||
controller_(controller_ptr), tf_listener_ptr(tf_listener_ptr), state_(INITIALIZED),
|
||||
moving_(false), max_retries_(0), patience_(0), vel_pub_(vel_pub), current_goal_pub_(goal_pub),
|
||||
calling_duration_(boost::chrono::microseconds(static_cast<int>(1e6 / DEFAULT_CONTROLLER_FREQUENCY)))
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
// non-dynamically reconfigurable parameters
|
||||
private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
|
||||
private_nh.param("map_frame", global_frame_, std::string("map"));
|
||||
private_nh.param("force_stop_at_goal", force_stop_at_goal_, false);
|
||||
private_nh.param("force_stop_on_cancel", force_stop_on_cancel_, false);
|
||||
private_nh.param("check_goal_reached", mbf_check_goal_reached_, false);
|
||||
private_nh.param("dist_tolerance", dist_tolerance_, 0.1);
|
||||
private_nh.param("angle_tolerance", angle_tolerance_, M_PI / 18.0);
|
||||
private_nh.param("tf_timeout", tf_timeout_, 1.0);
|
||||
|
||||
// dynamically reconfigurable parameters
|
||||
reconfigure(config);
|
||||
}
|
||||
|
||||
AbstractControllerExecution::~AbstractControllerExecution()
|
||||
{
|
||||
}
|
||||
|
||||
bool AbstractControllerExecution::setControllerFrequency(double frequency)
|
||||
{
|
||||
// set the calling duration by the moving frequency
|
||||
if (frequency <= 0.0)
|
||||
{
|
||||
ROS_ERROR("Controller frequency must be greater than 0.0! No change of the frequency!");
|
||||
return false;
|
||||
}
|
||||
calling_duration_ = boost::chrono::microseconds(static_cast<int>(1e6 / frequency));
|
||||
return true;
|
||||
}
|
||||
|
||||
void AbstractControllerExecution::reconfigure(const MoveBaseFlexConfig &config)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(configuration_mutex_);
|
||||
// Timeout granted to the controller. We keep calling it up to this time or up to max_retries times
|
||||
// If it doesn't return within time, the navigator will cancel it and abort the corresponding action
|
||||
patience_ = ros::Duration(config.controller_patience);
|
||||
|
||||
setControllerFrequency(config.controller_frequency);
|
||||
|
||||
max_retries_ = config.controller_max_retries;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractControllerExecution::start()
|
||||
{
|
||||
setState(STARTED);
|
||||
if (moving_)
|
||||
{
|
||||
return false; // thread is already running.
|
||||
}
|
||||
moving_ = true;
|
||||
return AbstractExecutionBase::start();
|
||||
}
|
||||
|
||||
|
||||
void AbstractControllerExecution::setState(ControllerState state)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(state_mtx_);
|
||||
state_ = state;
|
||||
}
|
||||
|
||||
|
||||
typename AbstractControllerExecution::ControllerState
|
||||
AbstractControllerExecution::getState()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(state_mtx_);
|
||||
return state_;
|
||||
}
|
||||
|
||||
void AbstractControllerExecution::setNewPlan(
|
||||
const std::vector<geometry_msgs::PoseStamped> &plan,
|
||||
bool tolerance_from_action,
|
||||
double action_dist_tolerance,
|
||||
double action_angle_tolerance)
|
||||
{
|
||||
if (moving_)
|
||||
{
|
||||
// This is fine on continuous replanning
|
||||
ROS_DEBUG("Setting new plan while moving");
|
||||
}
|
||||
boost::lock_guard<boost::mutex> guard(plan_mtx_);
|
||||
new_plan_ = true;
|
||||
|
||||
plan_ = plan;
|
||||
tolerance_from_action_ = tolerance_from_action;
|
||||
action_dist_tolerance_ = action_dist_tolerance;
|
||||
action_angle_tolerance_ = action_angle_tolerance;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractControllerExecution::hasNewPlan()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(plan_mtx_);
|
||||
return new_plan_;
|
||||
}
|
||||
|
||||
|
||||
std::vector<geometry_msgs::PoseStamped> AbstractControllerExecution::getNewPlan()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(plan_mtx_);
|
||||
new_plan_ = false;
|
||||
return plan_;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractControllerExecution::computeRobotPose()
|
||||
{
|
||||
if (!mbf_utility::getRobotPose(*tf_listener_ptr, robot_frame_, global_frame_,
|
||||
ros::Duration(tf_timeout_), robot_pose_))
|
||||
{
|
||||
ROS_ERROR_STREAM("Could not get the robot pose in the global frame. - robot frame: \""
|
||||
<< robot_frame_ << "\" global frame: \"" << global_frame_);
|
||||
message_ = "Could not get the robot pose";
|
||||
outcome_ = mbf_msgs::ExePathResult::TF_ERROR;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
uint32_t AbstractControllerExecution::computeVelocityCmd(const geometry_msgs::PoseStamped &robot_pose,
|
||||
const geometry_msgs::TwistStamped &robot_velocity,
|
||||
geometry_msgs::TwistStamped &vel_cmd,
|
||||
std::string &message)
|
||||
{
|
||||
return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
|
||||
}
|
||||
|
||||
|
||||
void AbstractControllerExecution::setVelocityCmd(const geometry_msgs::TwistStamped &vel_cmd)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(vel_cmd_mtx_);
|
||||
vel_cmd_stamped_ = vel_cmd;
|
||||
if (vel_cmd_stamped_.header.stamp.isZero())
|
||||
vel_cmd_stamped_.header.stamp = ros::Time::now();
|
||||
// TODO what happen with frame id?
|
||||
// TODO Add a queue here for handling the outcome, message and cmd_vel values bundled,
|
||||
// TODO so there should be no loss of information in the feedback stream
|
||||
}
|
||||
|
||||
|
||||
geometry_msgs::TwistStamped AbstractControllerExecution::getVelocityCmd()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(vel_cmd_mtx_);
|
||||
return vel_cmd_stamped_;
|
||||
}
|
||||
|
||||
|
||||
ros::Time AbstractControllerExecution::getLastPluginCallTime()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(lct_mtx_);
|
||||
return last_call_time_;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractControllerExecution::isPatienceExceeded()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(lct_mtx_);
|
||||
if(!patience_.isZero() && ros::Time::now() - start_time_ > patience_) // not zero -> activated, start_time handles init case
|
||||
{
|
||||
if(ros::Time::now() - last_call_time_ > patience_)
|
||||
{
|
||||
ROS_WARN_STREAM_THROTTLE(3, "The controller plugin \"" << name_ << "\" needs more time to compute in one run than the patience time!");
|
||||
return true;
|
||||
}
|
||||
if(ros::Time::now() - last_valid_cmd_time_ > patience_)
|
||||
{
|
||||
ROS_DEBUG_STREAM("The controller plugin \"" << name_ << "\" does not return a success state (outcome < 10) for more than the patience time in multiple runs!");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractControllerExecution::isMoving()
|
||||
{
|
||||
return moving_;
|
||||
}
|
||||
|
||||
bool AbstractControllerExecution::reachedGoalCheck()
|
||||
{
|
||||
// Use action-provided tolerances if requested, or use parameter values otherwise
|
||||
double dist_tolerance = tolerance_from_action_ ? action_dist_tolerance_ : dist_tolerance_;
|
||||
double angle_tolerance = tolerance_from_action_ ? action_angle_tolerance_ : angle_tolerance_;
|
||||
|
||||
if (mbf_check_goal_reached_)
|
||||
{
|
||||
// MBF checks if we have reached the goal, or...
|
||||
return mbf_utility::distance(robot_pose_, plan_.back()) < dist_tolerance &&
|
||||
mbf_utility::angle(robot_pose_, plan_.back()) < angle_tolerance;
|
||||
}
|
||||
// ...we let the controller decide
|
||||
return controller_->isGoalReached(dist_tolerance, angle_tolerance);
|
||||
}
|
||||
|
||||
bool AbstractControllerExecution::cancel()
|
||||
{
|
||||
// request the controller to cancel; it returns false if cancel is not implemented or rejected by the plugin
|
||||
if (!controller_->cancel())
|
||||
{
|
||||
ROS_WARN_STREAM("Cancel controlling failed. Wait until the current control cycle finished!");
|
||||
}
|
||||
// then wait for the control cycle to stop (should happen immediately if the controller cancel returned true)
|
||||
cancel_ = true;
|
||||
if (waitForStateUpdate(boost::chrono::milliseconds(500)) == boost::cv_status::timeout)
|
||||
{
|
||||
// this situation should never happen; if it does, the action server will be unready for goals immediately sent
|
||||
ROS_WARN_STREAM("Timeout while waiting for control cycle to stop; immediately sent goals can get stuck");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void AbstractControllerExecution::run()
|
||||
{
|
||||
start_time_ = ros::Time::now();
|
||||
|
||||
// init plan
|
||||
std::vector<geometry_msgs::PoseStamped> plan;
|
||||
if (!hasNewPlan())
|
||||
{
|
||||
setState(NO_PLAN);
|
||||
moving_ = false;
|
||||
ROS_ERROR("robot navigation moving has no plan!");
|
||||
}
|
||||
|
||||
last_valid_cmd_time_ = ros::Time();
|
||||
int retries = 0;
|
||||
int seq = 0;
|
||||
|
||||
try
|
||||
{
|
||||
while (moving_ && ros::ok())
|
||||
{
|
||||
boost::chrono::thread_clock::time_point loop_start_time = boost::chrono::thread_clock::now();
|
||||
|
||||
if (cancel_)
|
||||
{
|
||||
if (force_stop_on_cancel_)
|
||||
{
|
||||
publishZeroVelocity(); // command the robot to stop on canceling navigation
|
||||
}
|
||||
setState(CANCELED);
|
||||
condition_.notify_all();
|
||||
moving_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!safetyCheck())
|
||||
{
|
||||
// the specific implementation must have detected a risk situation; at this abstract level, we
|
||||
// cannot tell what the problem is, but anyway we command the robot to stop to avoid crashes
|
||||
publishZeroVelocity(); // note that we still feedback command calculated by the plugin
|
||||
boost::this_thread::sleep_for(calling_duration_);
|
||||
}
|
||||
|
||||
// update plan dynamically
|
||||
if (hasNewPlan())
|
||||
{
|
||||
plan = getNewPlan();
|
||||
|
||||
// check if plan is empty
|
||||
if (plan.empty())
|
||||
{
|
||||
setState(EMPTY_PLAN);
|
||||
condition_.notify_all();
|
||||
moving_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// check if plan could be set
|
||||
if (!controller_->setPlan(plan))
|
||||
{
|
||||
setState(INVALID_PLAN);
|
||||
condition_.notify_all();
|
||||
moving_ = false;
|
||||
return;
|
||||
}
|
||||
current_goal_pub_.publish(plan.back());
|
||||
}
|
||||
|
||||
// compute robot pose and store it in robot_pose_
|
||||
if (!computeRobotPose())
|
||||
{
|
||||
publishZeroVelocity();
|
||||
setState(INTERNAL_ERROR);
|
||||
condition_.notify_all();
|
||||
moving_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// ask planner if the goal is reached
|
||||
if (reachedGoalCheck())
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED("abstract_controller_execution", "Reached the goal!");
|
||||
if (force_stop_at_goal_)
|
||||
{
|
||||
publishZeroVelocity();
|
||||
}
|
||||
setState(ARRIVED_GOAL);
|
||||
// goal reached, tell it the controller
|
||||
condition_.notify_all();
|
||||
moving_ = false;
|
||||
// if not, keep moving
|
||||
}
|
||||
else
|
||||
{
|
||||
setState(PLANNING);
|
||||
|
||||
// save time and call the plugin
|
||||
lct_mtx_.lock();
|
||||
last_call_time_ = ros::Time::now();
|
||||
lct_mtx_.unlock();
|
||||
|
||||
// call plugin to compute the next velocity command
|
||||
geometry_msgs::TwistStamped cmd_vel_stamped;
|
||||
geometry_msgs::TwistStamped robot_velocity; // TODO pass current velocity to the plugin!
|
||||
outcome_ = computeVelocityCmd(robot_pose_, robot_velocity, cmd_vel_stamped, message_ = "");
|
||||
|
||||
if (outcome_ < 10)
|
||||
{
|
||||
setState(GOT_LOCAL_CMD);
|
||||
vel_pub_.publish(cmd_vel_stamped.twist);
|
||||
last_valid_cmd_time_ = ros::Time::now();
|
||||
retries = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(configuration_mutex_);
|
||||
if (max_retries_ >= 0 && ++retries > max_retries_)
|
||||
{
|
||||
setState(MAX_RETRIES);
|
||||
moving_ = false;
|
||||
}
|
||||
else if (isPatienceExceeded())
|
||||
{
|
||||
// patience limit enabled and running controller for more than patience without valid commands
|
||||
setState(PAT_EXCEEDED);
|
||||
moving_ = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
setState(NO_LOCAL_CMD); // useful for server feedback
|
||||
}
|
||||
// could not compute a valid velocity command -> stop moving the robot
|
||||
publishZeroVelocity(); // command the robot to stop; we still feedback command calculated by the plugin
|
||||
}
|
||||
|
||||
// set stamped values; timestamp and frame_id should be set by the plugin; otherwise setVelocityCmd will do
|
||||
cmd_vel_stamped.header.seq = seq++; // sequence number
|
||||
setVelocityCmd(cmd_vel_stamped);
|
||||
condition_.notify_all();
|
||||
}
|
||||
|
||||
boost::chrono::thread_clock::time_point end_time = boost::chrono::thread_clock::now();
|
||||
boost::chrono::microseconds execution_duration =
|
||||
boost::chrono::duration_cast<boost::chrono::microseconds>(end_time - loop_start_time);
|
||||
configuration_mutex_.lock();
|
||||
boost::chrono::microseconds sleep_time = calling_duration_ - execution_duration;
|
||||
configuration_mutex_.unlock();
|
||||
if (moving_ && ros::ok())
|
||||
{
|
||||
if (sleep_time > boost::chrono::microseconds(0))
|
||||
{
|
||||
// interruption point
|
||||
boost::this_thread::sleep_for(sleep_time);
|
||||
}
|
||||
else
|
||||
{
|
||||
// provide an interruption point also with 0 or negative sleep_time
|
||||
boost::this_thread::interruption_point();
|
||||
ROS_WARN_THROTTLE(1.0, "Calculation needs too much time to stay in the moving frequency! (%f > %f)",
|
||||
execution_duration.count()/1000000.0, calling_duration_.count()/1000000.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (const boost::thread_interrupted &ex)
|
||||
{
|
||||
// Controller thread interrupted; in most cases we have started a new plan
|
||||
// Can also be that robot is oscillating or we have exceeded planner patience
|
||||
ROS_DEBUG_STREAM("Controller thread interrupted!");
|
||||
publishZeroVelocity();
|
||||
setState(STOPPED);
|
||||
condition_.notify_all();
|
||||
moving_ = false;
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
message_ = "Unknown error occurred: " + boost::current_exception_diagnostic_information();
|
||||
ROS_FATAL_STREAM(message_);
|
||||
setState(INTERNAL_ERROR);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AbstractControllerExecution::publishZeroVelocity()
|
||||
{
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
cmd_vel.linear.x = 0;
|
||||
cmd_vel.linear.y = 0;
|
||||
cmd_vel.linear.z = 0;
|
||||
cmd_vel.angular.x = 0;
|
||||
cmd_vel.angular.y = 0;
|
||||
cmd_vel.angular.z = 0;
|
||||
vel_pub_.publish(cmd_vel);
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
88
navigations/move_base_flex/mbf_abstract_nav/src/abstract_execution_base.cpp
Executable file
88
navigations/move_base_flex/mbf_abstract_nav/src/abstract_execution_base.cpp
Executable file
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Copyright 2018, Sebastian Pütz
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
* abstract_execution_base.cpp
|
||||
*
|
||||
* author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mbf_abstract_nav/abstract_execution_base.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
AbstractExecutionBase::AbstractExecutionBase(std::string name)
|
||||
: outcome_(255), cancel_(false), name_(name)
|
||||
{
|
||||
}
|
||||
|
||||
bool AbstractExecutionBase::start()
|
||||
{
|
||||
thread_ = boost::thread(&AbstractExecutionBase::run, this);
|
||||
return true;
|
||||
}
|
||||
|
||||
void AbstractExecutionBase::stop()
|
||||
{
|
||||
ROS_WARN_STREAM("Try to stop the plugin \"" << name_ << "\" rigorously by interrupting the thread!");
|
||||
thread_.interrupt();
|
||||
}
|
||||
|
||||
void AbstractExecutionBase::join(){
|
||||
if (thread_.joinable())
|
||||
thread_.join();
|
||||
}
|
||||
|
||||
boost::cv_status AbstractExecutionBase::waitForStateUpdate(boost::chrono::microseconds const &duration)
|
||||
{
|
||||
boost::mutex mutex;
|
||||
boost::unique_lock<boost::mutex> lock(mutex);
|
||||
return condition_.wait_for(lock, duration);
|
||||
}
|
||||
|
||||
uint32_t AbstractExecutionBase::getOutcome()
|
||||
{
|
||||
return outcome_;
|
||||
}
|
||||
|
||||
std::string AbstractExecutionBase::getMessage()
|
||||
{
|
||||
return message_;
|
||||
}
|
||||
|
||||
std::string AbstractExecutionBase::getName()
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
382
navigations/move_base_flex/mbf_abstract_nav/src/abstract_navigation_server.cpp
Executable file
382
navigations/move_base_flex/mbf_abstract_nav/src/abstract_navigation_server.cpp
Executable file
@@ -0,0 +1,382 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* abstract_navigation_server.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nav_msgs/Path.h>
|
||||
|
||||
#include "mbf_abstract_nav/abstract_navigation_server.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
|
||||
: tf_listener_ptr_(tf_listener_ptr), private_nh_("~"),
|
||||
planner_plugin_manager_("planners",
|
||||
boost::bind(&AbstractNavigationServer::loadPlannerPlugin, this, _1),
|
||||
boost::bind(&AbstractNavigationServer::initializePlannerPlugin, this, _1, _2)),
|
||||
controller_plugin_manager_("controllers",
|
||||
boost::bind(&AbstractNavigationServer::loadControllerPlugin, this, _1),
|
||||
boost::bind(&AbstractNavigationServer::initializeControllerPlugin, this, _1, _2)),
|
||||
recovery_plugin_manager_("recovery_behaviors",
|
||||
boost::bind(&AbstractNavigationServer::loadRecoveryPlugin, this, _1),
|
||||
boost::bind(&AbstractNavigationServer::initializeRecoveryPlugin, this, _1, _2)),
|
||||
tf_timeout_(private_nh_.param<double>("tf_timeout", 3.0)),
|
||||
global_frame_(private_nh_.param<std::string>("global_frame", "map")),
|
||||
robot_frame_(private_nh_.param<std::string>("robot_frame", "base_link")),
|
||||
robot_info_(*tf_listener_ptr, global_frame_, robot_frame_, tf_timeout_),
|
||||
controller_action_(name_action_exe_path, robot_info_),
|
||||
planner_action_(name_action_get_path, robot_info_),
|
||||
recovery_action_(name_action_recovery, robot_info_),
|
||||
move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames())
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
|
||||
goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
|
||||
|
||||
// init cmd_vel publisher for the robot velocity
|
||||
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
||||
|
||||
action_server_get_path_ptr_ = ActionServerGetPathPtr(
|
||||
new ActionServerGetPath(
|
||||
private_nh_,
|
||||
name_action_get_path,
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionGetPath, this, _1),
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionGetPath, this, _1),
|
||||
false));
|
||||
|
||||
action_server_exe_path_ptr_ = ActionServerExePathPtr(
|
||||
new ActionServerExePath(
|
||||
private_nh_,
|
||||
name_action_exe_path,
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionExePath, this, _1),
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionExePath, this, _1),
|
||||
false));
|
||||
|
||||
action_server_recovery_ptr_ = ActionServerRecoveryPtr(
|
||||
new ActionServerRecovery(
|
||||
private_nh_,
|
||||
name_action_recovery,
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionRecovery, this, _1),
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionRecovery, this, _1),
|
||||
false));
|
||||
|
||||
action_server_move_base_ptr_ = ActionServerMoveBasePtr(
|
||||
new ActionServerMoveBase(
|
||||
private_nh_,
|
||||
name_action_move_base,
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionMoveBase, this, _1),
|
||||
boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1),
|
||||
false));
|
||||
|
||||
// XXX note that we don't start a dynamic reconfigure server, to avoid colliding with the one possibly created by
|
||||
// the base class. If none, it should call startDynamicReconfigureServer method to start the one defined here for
|
||||
// providing just the abstract server parameters
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::initializeServerComponents()
|
||||
{
|
||||
planner_plugin_manager_.loadPlugins();
|
||||
controller_plugin_manager_.loadPlugins();
|
||||
recovery_plugin_manager_.loadPlugins();
|
||||
}
|
||||
|
||||
AbstractNavigationServer::~AbstractNavigationServer()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::callActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
|
||||
{
|
||||
const mbf_msgs::GetPathGoal &goal = *(goal_handle.getGoal().get());
|
||||
const geometry_msgs::Point &p = goal.target_pose.pose.position;
|
||||
|
||||
std::string planner_name;
|
||||
if(!planner_plugin_manager_.getLoadedNames().empty())
|
||||
{
|
||||
planner_name = goal.planner.empty() ? planner_plugin_manager_.getLoadedNames().front() : goal.planner;
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::GetPathResult result;
|
||||
result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
|
||||
result.message = "No plugins loaded at all!";
|
||||
ROS_WARN_STREAM_NAMED("get_path", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
if(!planner_plugin_manager_.hasPlugin(planner_name))
|
||||
{
|
||||
mbf_msgs::GetPathResult result;
|
||||
result.outcome = mbf_msgs::GetPathResult::INVALID_PLUGIN;
|
||||
result.message = "No plugin loaded with the given name \"" + goal.planner + "\"!";
|
||||
ROS_WARN_STREAM_NAMED("get_path", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
mbf_abstract_core::AbstractPlanner::Ptr planner_plugin = planner_plugin_manager_.getPlugin(planner_name);
|
||||
ROS_DEBUG_STREAM_NAMED("get_path", "Start action \"get_path\" using planner \"" << planner_name
|
||||
<< "\" of type \"" << planner_plugin_manager_.getType(planner_name) << "\"");
|
||||
|
||||
|
||||
if(planner_plugin)
|
||||
{
|
||||
mbf_abstract_nav::AbstractPlannerExecution::Ptr planner_execution
|
||||
= newPlannerExecution(planner_name, planner_plugin);
|
||||
|
||||
//start another planning action
|
||||
planner_action_.start(goal_handle, planner_execution);
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::GetPathResult result;
|
||||
result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
|
||||
result.message = "Internal Error: \"planner_plugin\" pointer should not be a null pointer!";
|
||||
ROS_FATAL_STREAM_NAMED("get_path", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
}
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::cancelActionGetPath(ActionServerGetPath::GoalHandle goal_handle)
|
||||
{
|
||||
ROS_INFO_STREAM_NAMED("get_path", "Cancel action \"get_path\"");
|
||||
planner_action_.cancel(goal_handle);
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle goal_handle)
|
||||
{
|
||||
const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
|
||||
|
||||
std::string controller_name;
|
||||
if(!controller_plugin_manager_.getLoadedNames().empty())
|
||||
{
|
||||
controller_name = goal.controller.empty() ? controller_plugin_manager_.getLoadedNames().front() : goal.controller;
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::ExePathResult result;
|
||||
result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
|
||||
result.message = "No plugins loaded at all!";
|
||||
ROS_WARN_STREAM_NAMED("exe_path", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
if(!controller_plugin_manager_.hasPlugin(controller_name))
|
||||
{
|
||||
mbf_msgs::ExePathResult result;
|
||||
result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN;
|
||||
result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!";
|
||||
ROS_WARN_STREAM_NAMED("exe_path", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
mbf_abstract_core::AbstractController::Ptr controller_plugin = controller_plugin_manager_.getPlugin(controller_name);
|
||||
ROS_DEBUG_STREAM_NAMED("exe_path", "Start action \"exe_path\" using controller \"" << controller_name
|
||||
<< "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\"");
|
||||
|
||||
|
||||
if(controller_plugin)
|
||||
{
|
||||
mbf_abstract_nav::AbstractControllerExecution::Ptr controller_execution
|
||||
= newControllerExecution(controller_name, controller_plugin);
|
||||
|
||||
// starts another controller action
|
||||
controller_action_.start(goal_handle, controller_execution);
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::ExePathResult result;
|
||||
result.outcome = mbf_msgs::ExePathResult::INTERNAL_ERROR;
|
||||
result.message = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!";
|
||||
ROS_FATAL_STREAM_NAMED("exe_path", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
}
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHandle goal_handle)
|
||||
{
|
||||
ROS_INFO_STREAM_NAMED("exe_path", "Cancel action \"exe_path\"");
|
||||
controller_action_.cancel(goal_handle);
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::callActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
|
||||
{
|
||||
const mbf_msgs::RecoveryGoal &goal = *(goal_handle.getGoal().get());
|
||||
|
||||
std::string recovery_name;
|
||||
|
||||
if(!recovery_plugin_manager_.getLoadedNames().empty())
|
||||
{
|
||||
recovery_name = goal.behavior.empty() ? recovery_plugin_manager_.getLoadedNames().front() : goal.behavior;
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::RecoveryResult result;
|
||||
result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
|
||||
result.message = "No plugins loaded at all!";
|
||||
ROS_WARN_STREAM_NAMED("recovery", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
if(!recovery_plugin_manager_.hasPlugin(recovery_name))
|
||||
{
|
||||
mbf_msgs::RecoveryResult result;
|
||||
result.outcome = mbf_msgs::RecoveryResult::INVALID_PLUGIN;
|
||||
result.message = "No plugin loaded with the given name \"" + goal.behavior + "\"!";
|
||||
ROS_WARN_STREAM_NAMED("recovery", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
mbf_abstract_core::AbstractRecovery::Ptr recovery_plugin = recovery_plugin_manager_.getPlugin(recovery_name);
|
||||
ROS_DEBUG_STREAM_NAMED("recovery", "Start action \"recovery\" using recovery \"" << recovery_name
|
||||
<< "\" of type \"" << recovery_plugin_manager_.getType(recovery_name) << "\"");
|
||||
|
||||
|
||||
if(recovery_plugin)
|
||||
{
|
||||
mbf_abstract_nav::AbstractRecoveryExecution::Ptr recovery_execution
|
||||
= newRecoveryExecution(recovery_name, recovery_plugin);
|
||||
|
||||
recovery_action_.start(goal_handle, recovery_execution);
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::RecoveryResult result;
|
||||
result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
|
||||
result.message = "Internal Error: \"recovery_plugin\" pointer should not be a null pointer!";
|
||||
ROS_FATAL_STREAM_NAMED("recovery", result.message);
|
||||
goal_handle.setRejected(result, result.message);
|
||||
}
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::cancelActionRecovery(ActionServerRecovery::GoalHandle goal_handle)
|
||||
{
|
||||
ROS_INFO_STREAM_NAMED("recovery", "Cancel action \"recovery\"");
|
||||
recovery_action_.cancel(goal_handle);
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
|
||||
move_base_action_.start(goal_handle);
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle)
|
||||
{
|
||||
ROS_INFO_STREAM_NAMED("move_base", "Cancel action \"move_base\"");
|
||||
move_base_action_.cancel();
|
||||
}
|
||||
|
||||
mbf_abstract_nav::AbstractPlannerExecution::Ptr AbstractNavigationServer::newPlannerExecution(
|
||||
const std::string &plugin_name,
|
||||
const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr)
|
||||
{
|
||||
return boost::make_shared<mbf_abstract_nav::AbstractPlannerExecution>(plugin_name, plugin_ptr, last_config_);
|
||||
}
|
||||
|
||||
mbf_abstract_nav::AbstractControllerExecution::Ptr AbstractNavigationServer::newControllerExecution(
|
||||
const std::string &plugin_name,
|
||||
const mbf_abstract_core::AbstractController::Ptr &plugin_ptr)
|
||||
{
|
||||
return boost::make_shared<mbf_abstract_nav::AbstractControllerExecution>(plugin_name, plugin_ptr, vel_pub_, goal_pub_,
|
||||
tf_listener_ptr_, last_config_);
|
||||
}
|
||||
|
||||
mbf_abstract_nav::AbstractRecoveryExecution::Ptr AbstractNavigationServer::newRecoveryExecution(
|
||||
const std::string &plugin_name,
|
||||
const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr)
|
||||
{
|
||||
return boost::make_shared<mbf_abstract_nav::AbstractRecoveryExecution>(plugin_name, plugin_ptr,
|
||||
tf_listener_ptr_, last_config_);
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::startActionServers()
|
||||
{
|
||||
action_server_get_path_ptr_->start();
|
||||
action_server_exe_path_ptr_->start();
|
||||
action_server_recovery_ptr_->start();
|
||||
action_server_move_base_ptr_->start();
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::startDynamicReconfigureServer()
|
||||
{
|
||||
// dynamic reconfigure server
|
||||
dsrv_ = boost::make_shared<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBaseFlexConfig> >(private_nh_);
|
||||
dsrv_->setCallback(boost::bind(&AbstractNavigationServer::reconfigure, this, _1, _2));
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::reconfigure(
|
||||
mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(configuration_mutex_);
|
||||
|
||||
// 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;
|
||||
}
|
||||
planner_action_.reconfigureAll(config, level);
|
||||
controller_action_.reconfigureAll(config, level);
|
||||
recovery_action_.reconfigureAll(config, level);
|
||||
move_base_action_.reconfigure(config, level);
|
||||
|
||||
last_config_ = config;
|
||||
}
|
||||
|
||||
void AbstractNavigationServer::stop(){
|
||||
planner_action_.cancelAll();
|
||||
controller_action_.cancelAll();
|
||||
recovery_action_.cancelAll();
|
||||
move_base_action_.cancel();
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
358
navigations/move_base_flex/mbf_abstract_nav/src/abstract_planner_execution.cpp
Executable file
358
navigations/move_base_flex/mbf_abstract_nav/src/abstract_planner_execution.cpp
Executable file
@@ -0,0 +1,358 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* abstract_planner_execution.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mbf_abstract_nav/abstract_planner_execution.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
AbstractPlannerExecution::AbstractPlannerExecution(
|
||||
const std::string &name,
|
||||
const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr,
|
||||
const MoveBaseFlexConfig &config) :
|
||||
AbstractExecutionBase(name),
|
||||
planner_(planner_ptr), state_(INITIALIZED), planning_(false),
|
||||
has_new_start_(false), has_new_goal_(false)
|
||||
{
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
// non-dynamically reconfigurable parameters
|
||||
private_nh.param("robot_frame", robot_frame_, std::string("base_footprint"));
|
||||
private_nh.param("map_frame", global_frame_, std::string("map"));
|
||||
|
||||
// dynamically reconfigurable parameters
|
||||
reconfigure(config);
|
||||
}
|
||||
|
||||
AbstractPlannerExecution::~AbstractPlannerExecution()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
double AbstractPlannerExecution::getCost() const
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(plan_mtx_);
|
||||
// copy plan and costs to output
|
||||
// if the planner plugin do not compute costs compute costs by discrete path length
|
||||
if(cost_ == 0 && !plan_.empty())
|
||||
{
|
||||
ROS_DEBUG_STREAM("Compute costs by discrete path length!");
|
||||
double cost = 0;
|
||||
|
||||
geometry_msgs::PoseStamped prev_pose = plan_.front();
|
||||
for(std::vector<geometry_msgs::PoseStamped>::const_iterator iter = plan_.begin() + 1; iter != plan_.end(); ++iter)
|
||||
{
|
||||
cost += mbf_utility::distance(prev_pose, *iter);
|
||||
prev_pose = *iter;
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
return cost_;
|
||||
}
|
||||
|
||||
void AbstractPlannerExecution::reconfigure(const MoveBaseFlexConfig &config)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(configuration_mutex_);
|
||||
|
||||
max_retries_ = config.planner_max_retries;
|
||||
frequency_ = config.planner_frequency;
|
||||
|
||||
// Timeout granted to the global planner. We keep calling it up to this time or up to max_retries times
|
||||
// If it doesn't return within time, the navigator will cancel it and abort the corresponding action
|
||||
patience_ = ros::Duration(config.planner_patience);
|
||||
}
|
||||
|
||||
|
||||
typename AbstractPlannerExecution::PlanningState AbstractPlannerExecution::getState() const
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(state_mtx_);
|
||||
return state_;
|
||||
}
|
||||
|
||||
void AbstractPlannerExecution::setState(PlanningState state, bool signalling)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(state_mtx_);
|
||||
state_ = state;
|
||||
|
||||
// some states are quiet, most aren't
|
||||
if(signalling)
|
||||
condition_.notify_all();
|
||||
}
|
||||
|
||||
|
||||
ros::Time AbstractPlannerExecution::getLastValidPlanTime() const
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(plan_mtx_);
|
||||
return last_valid_plan_time_;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractPlannerExecution::isPatienceExceeded() const
|
||||
{
|
||||
return !patience_.isZero() && (ros::Time::now() - last_call_start_time_ > patience_);
|
||||
}
|
||||
|
||||
|
||||
std::vector<geometry_msgs::PoseStamped> AbstractPlannerExecution::getPlan() const
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(plan_mtx_);
|
||||
// copy plan and costs to output
|
||||
return plan_;
|
||||
}
|
||||
|
||||
|
||||
void AbstractPlannerExecution::setNewGoal(const geometry_msgs::PoseStamped &goal,
|
||||
double dist_tolerance, double angle_tolerance)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
|
||||
goal_ = goal;
|
||||
dist_tolerance_ = dist_tolerance;
|
||||
angle_tolerance_ = angle_tolerance;
|
||||
has_new_goal_ = true;
|
||||
}
|
||||
|
||||
|
||||
void AbstractPlannerExecution::setNewStart(const geometry_msgs::PoseStamped &start)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
|
||||
start_ = start;
|
||||
has_new_start_ = true;
|
||||
}
|
||||
|
||||
|
||||
void AbstractPlannerExecution::setNewStartAndGoal(const geometry_msgs::PoseStamped &start,
|
||||
const geometry_msgs::PoseStamped &goal,
|
||||
double dist_tolerance, double angle_tolerance)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(goal_start_mtx_);
|
||||
start_ = start;
|
||||
goal_ = goal;
|
||||
dist_tolerance_ = dist_tolerance;
|
||||
angle_tolerance_ = angle_tolerance;
|
||||
has_new_start_ = true;
|
||||
has_new_goal_ = true;
|
||||
}
|
||||
|
||||
|
||||
bool AbstractPlannerExecution::start(const geometry_msgs::PoseStamped &start,
|
||||
const geometry_msgs::PoseStamped &goal,
|
||||
double dist_tolerance, double angle_tolerance)
|
||||
{
|
||||
if (planning_)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
boost::lock_guard<boost::mutex> guard(planning_mtx_);
|
||||
planning_ = true;
|
||||
start_ = start;
|
||||
goal_ = goal;
|
||||
dist_tolerance_ = dist_tolerance;
|
||||
angle_tolerance_ = angle_tolerance;
|
||||
|
||||
const geometry_msgs::Point& s = start.pose.position;
|
||||
const geometry_msgs::Point& g = goal.pose.position;
|
||||
|
||||
ROS_DEBUG_STREAM("Start planning from the start pose: (" << s.x << ", " << s.y << ", " << s.z << ")"
|
||||
<< " to the goal pose: ("<< g.x << ", " << g.y << ", " << g.z << ")");
|
||||
|
||||
return AbstractExecutionBase::start();
|
||||
}
|
||||
|
||||
|
||||
bool AbstractPlannerExecution::cancel()
|
||||
{
|
||||
cancel_ = true; // force cancel immediately, as the call to cancel in the planner can take a while
|
||||
|
||||
// returns false if cancel is not implemented or rejected by the planner (will run until completion)
|
||||
if (!planner_->cancel())
|
||||
{
|
||||
ROS_WARN_STREAM("Cancel planning failed or is not supported by the plugin. "
|
||||
<< "Wait until the current planning finished!");
|
||||
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t AbstractPlannerExecution::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)
|
||||
{
|
||||
return planner_->makePlan(start, goal, dist_tolerance, angle_tolerance, plan, cost, message);
|
||||
}
|
||||
|
||||
void AbstractPlannerExecution::run()
|
||||
{
|
||||
setState(STARTED, false);
|
||||
boost::lock_guard<boost::mutex> guard(planning_mtx_);
|
||||
int retries = 0;
|
||||
geometry_msgs::PoseStamped current_start = start_;
|
||||
geometry_msgs::PoseStamped current_goal = goal_;
|
||||
|
||||
bool success = false;
|
||||
bool make_plan = false;
|
||||
bool exceeded = false;
|
||||
|
||||
last_call_start_time_ = ros::Time::now();
|
||||
last_valid_plan_time_ = ros::Time::now();
|
||||
|
||||
try
|
||||
{
|
||||
while (planning_ && ros::ok())
|
||||
{
|
||||
boost::chrono::thread_clock::time_point start_time = boost::chrono::thread_clock::now();
|
||||
|
||||
// call the planner
|
||||
std::vector<geometry_msgs::PoseStamped> plan;
|
||||
double cost;
|
||||
|
||||
// lock goal start mutex
|
||||
goal_start_mtx_.lock();
|
||||
if (has_new_start_)
|
||||
{
|
||||
has_new_start_ = false;
|
||||
current_start = start_;
|
||||
ROS_INFO_STREAM("A new start pose is available. Planning with the new start pose!");
|
||||
exceeded = false;
|
||||
const geometry_msgs::Point& s = start_.pose.position;
|
||||
ROS_INFO_STREAM("New planning start pose: (" << s.x << ", " << s.y << ", " << s.z << ")");
|
||||
}
|
||||
if (has_new_goal_)
|
||||
{
|
||||
has_new_goal_ = false;
|
||||
current_goal = goal_;
|
||||
ROS_INFO_STREAM("A new goal pose is available. Planning with the new goal pose and tolerances: "
|
||||
<< dist_tolerance_ << " m, " << angle_tolerance_ << " rad");
|
||||
exceeded = false;
|
||||
const geometry_msgs::Point& g = goal_.pose.position;
|
||||
ROS_INFO_STREAM("New goal pose: (" << g.x << ", " << g.y << ", " << g.z << ")");
|
||||
}
|
||||
|
||||
make_plan = !(success || exceeded) || has_new_start_ || has_new_goal_;
|
||||
|
||||
// unlock goal
|
||||
goal_start_mtx_.unlock();
|
||||
setState(PLANNING, false);
|
||||
if (make_plan)
|
||||
{
|
||||
outcome_ = makePlan(current_start, current_goal, dist_tolerance_, angle_tolerance_, plan, cost, message_);
|
||||
success = outcome_ < 10;
|
||||
|
||||
boost::lock_guard<boost::mutex> guard(configuration_mutex_);
|
||||
|
||||
if (cancel_ && !isPatienceExceeded())
|
||||
{
|
||||
ROS_INFO_STREAM("The planner \"" << name_ << "\" has been canceled!"); // but not due to patience exceeded
|
||||
planning_ = false;
|
||||
setState(CANCELED, true);
|
||||
}
|
||||
else if (success)
|
||||
{
|
||||
ROS_DEBUG_STREAM("Successfully found a plan.");
|
||||
exceeded = false;
|
||||
planning_ = false;
|
||||
|
||||
plan_mtx_.lock();
|
||||
plan_ = plan;
|
||||
// todo compute the cost once!
|
||||
cost_ = cost;
|
||||
last_valid_plan_time_ = ros::Time::now();
|
||||
plan_mtx_.unlock();
|
||||
setState(FOUND_PLAN, true);
|
||||
}
|
||||
else if (max_retries_ >= 0 && ++retries > max_retries_)
|
||||
{
|
||||
ROS_INFO_STREAM("Planning reached max retries! (" << max_retries_ << ")");
|
||||
exceeded = true;
|
||||
planning_ = false;
|
||||
setState(MAX_RETRIES, true);
|
||||
}
|
||||
else if (isPatienceExceeded())
|
||||
{
|
||||
// Patience exceeded is handled at two levels: here to stop retrying planning when max_retries is
|
||||
// disabled, and on the navigation server when the planner doesn't return for more that patience seconds.
|
||||
// In the second case, the navigation server has tried to cancel planning (possibly without success, as
|
||||
// old nav_core-based planners do not support canceling), and we add here the fact to the log for info
|
||||
ROS_INFO_STREAM("Planning patience (" << patience_.toSec() << "s) has been exceeded"
|
||||
<< (cancel_ ? "; planner canceled!" : ""));
|
||||
exceeded = true;
|
||||
planning_ = false;
|
||||
setState(PAT_EXCEEDED, true);
|
||||
}
|
||||
else if (max_retries_ == 0 && patience_.isZero())
|
||||
{
|
||||
ROS_INFO_STREAM("Planning could not find a plan!");
|
||||
exceeded = true;
|
||||
planning_ = false;
|
||||
setState(NO_PLAN_FOUND, true);
|
||||
}
|
||||
else
|
||||
{
|
||||
exceeded = false;
|
||||
ROS_DEBUG_STREAM("Planning could not find a plan! Trying again...");
|
||||
}
|
||||
}
|
||||
else if (cancel_)
|
||||
{
|
||||
ROS_INFO_STREAM("The global planner has been canceled!");
|
||||
planning_ = false;
|
||||
setState(CANCELED, true);
|
||||
}
|
||||
} // while (planning_ && ros::ok())
|
||||
}
|
||||
catch (const boost::thread_interrupted &ex)
|
||||
{
|
||||
// Planner thread interrupted; probably we have exceeded planner patience
|
||||
ROS_WARN_STREAM("Planner thread interrupted!");
|
||||
planning_ = false;
|
||||
setState(STOPPED, true);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
ROS_FATAL_STREAM("Unknown error occurred: " << boost::current_exception_diagnostic_information());
|
||||
setState(INTERNAL_ERROR, true);
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
|
||||
144
navigations/move_base_flex/mbf_abstract_nav/src/abstract_recovery_execution.cpp
Executable file
144
navigations/move_base_flex/mbf_abstract_nav/src/abstract_recovery_execution.cpp
Executable file
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* abstract_recovery_execution.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <boost/exception/diagnostic_information.hpp>
|
||||
|
||||
#include <mbf_abstract_nav/abstract_recovery_execution.h>
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
AbstractRecoveryExecution::AbstractRecoveryExecution(
|
||||
const std::string &name,
|
||||
const mbf_abstract_core::AbstractRecovery::Ptr &recovery_ptr,
|
||||
const TFPtr &tf_listener_ptr,
|
||||
const MoveBaseFlexConfig &config) :
|
||||
AbstractExecutionBase(name),
|
||||
behavior_(recovery_ptr), tf_listener_ptr_(tf_listener_ptr), state_(INITIALIZED)
|
||||
{
|
||||
// dynamically reconfigurable parameters
|
||||
reconfigure(config);
|
||||
}
|
||||
|
||||
AbstractRecoveryExecution::~AbstractRecoveryExecution()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void AbstractRecoveryExecution::reconfigure(const MoveBaseFlexConfig &config)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(conf_mtx_);
|
||||
|
||||
// Maximum time allowed to recovery behaviors. Intended as a safeward for the case a behavior hangs.
|
||||
// If it doesn't return within time, the navigator will cancel it and abort the corresponding action.
|
||||
patience_ = ros::Duration(config.recovery_patience);
|
||||
|
||||
// Nothing else to do here, as recovery_enabled is loaded and used in the navigation server
|
||||
}
|
||||
|
||||
|
||||
void AbstractRecoveryExecution::setState(RecoveryState state)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(state_mtx_);
|
||||
state_ = state;
|
||||
}
|
||||
|
||||
|
||||
typename AbstractRecoveryExecution::RecoveryState AbstractRecoveryExecution::getState()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(state_mtx_);
|
||||
return state_;
|
||||
}
|
||||
|
||||
bool AbstractRecoveryExecution::cancel()
|
||||
{
|
||||
cancel_ = true;
|
||||
// returns false if cancel is not implemented or rejected by the recovery behavior (will run until completion)
|
||||
if (!behavior_->cancel())
|
||||
{
|
||||
ROS_WARN_STREAM("Cancel recovery behavior \"" << name_ << "\" failed or is not supported by the plugin. "
|
||||
<< "Wait until the current recovery behavior finished!");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AbstractRecoveryExecution::isPatienceExceeded()
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard1(conf_mtx_);
|
||||
boost::lock_guard<boost::mutex> guard2(time_mtx_);
|
||||
ROS_DEBUG_STREAM("Patience: " << patience_ << ", start time: " << start_time_ << " now: " << ros::Time::now());
|
||||
return !patience_.isZero() && (ros::Time::now() - start_time_ > patience_);
|
||||
}
|
||||
|
||||
void AbstractRecoveryExecution::run()
|
||||
{
|
||||
cancel_ = false; // reset the canceled state
|
||||
|
||||
time_mtx_.lock();
|
||||
start_time_ = ros::Time::now();
|
||||
time_mtx_.unlock();
|
||||
setState(RECOVERING);
|
||||
try
|
||||
{
|
||||
outcome_ = behavior_->runBehavior(message_);
|
||||
if (cancel_)
|
||||
{
|
||||
setState(CANCELED);
|
||||
}
|
||||
else
|
||||
{
|
||||
setState(RECOVERY_DONE);
|
||||
}
|
||||
}
|
||||
catch (boost::thread_interrupted &ex)
|
||||
{
|
||||
ROS_WARN_STREAM("Recovery \"" << name_ << "\" interrupted!");
|
||||
setState(STOPPED);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
ROS_FATAL_STREAM("Unknown error occurred in recovery behavior \"" << name_ << "\": " << boost::current_exception_diagnostic_information());
|
||||
setState(INTERNAL_ERROR);
|
||||
}
|
||||
condition_.notify_one();
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
364
navigations/move_base_flex/mbf_abstract_nav/src/controller_action.cpp
Executable file
364
navigations/move_base_flex/mbf_abstract_nav/src/controller_action.cpp
Executable file
@@ -0,0 +1,364 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* controller_action.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mbf_abstract_nav/controller_action.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
ControllerAction::ControllerAction(
|
||||
const std::string &action_name,
|
||||
const mbf_utility::RobotInformation &robot_info)
|
||||
: AbstractActionBase(action_name, robot_info, boost::bind(&mbf_abstract_nav::ControllerAction::run, this, _1, _2))
|
||||
{
|
||||
}
|
||||
|
||||
void ControllerAction::start(
|
||||
GoalHandle &goal_handle,
|
||||
typename AbstractControllerExecution::Ptr execution_ptr
|
||||
)
|
||||
{
|
||||
if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING)
|
||||
{
|
||||
goal_handle.setCanceled();
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t slot = goal_handle.getGoal()->concurrency_slot;
|
||||
|
||||
bool update_plan = false;
|
||||
slot_map_mtx_.lock();
|
||||
std::map<uint8_t, ConcurrencySlot>::iterator slot_it = concurrency_slots_.find(slot);
|
||||
if(slot_it != concurrency_slots_.end() && slot_it->second.in_use)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> goal_guard(goal_mtx_);
|
||||
if(slot_it->second.execution->getName() == goal_handle.getGoal()->controller ||
|
||||
goal_handle.getGoal()->controller.empty())
|
||||
{
|
||||
update_plan = true;
|
||||
// Goal requests to run the same controller on the same concurrency slot already in use:
|
||||
// we update the goal handle and pass the new plan and tolerances from the action to the
|
||||
// execution without stopping it
|
||||
execution_ptr = slot_it->second.execution;
|
||||
execution_ptr->setNewPlan(goal_handle.getGoal()->path.poses,
|
||||
goal_handle.getGoal()->tolerance_from_action,
|
||||
goal_handle.getGoal()->dist_tolerance,
|
||||
goal_handle.getGoal()->angle_tolerance);
|
||||
// Update also goal pose, so the feedback remains consistent
|
||||
goal_pose_ = goal_handle.getGoal()->path.poses.back();
|
||||
mbf_msgs::ExePathResult result;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result);
|
||||
concurrency_slots_[slot].goal_handle.setCanceled(result, result.message);
|
||||
concurrency_slots_[slot].goal_handle = goal_handle;
|
||||
concurrency_slots_[slot].goal_handle.setAccepted();
|
||||
}
|
||||
}
|
||||
slot_map_mtx_.unlock();
|
||||
if(!update_plan)
|
||||
{
|
||||
// Otherwise run parent version of this method
|
||||
AbstractActionBase::start(goal_handle, execution_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution &execution)
|
||||
{
|
||||
goal_mtx_.lock();
|
||||
// Note that we always use the goal handle stored on the concurrency slots map, as it can change when replanning
|
||||
uint8_t slot = goal_handle.getGoal()->concurrency_slot;
|
||||
goal_mtx_.unlock();
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Start action " << name_);
|
||||
|
||||
// ensure we don't provide values from previous execution on case of error before filling both poses
|
||||
goal_pose_ = geometry_msgs::PoseStamped();
|
||||
robot_pose_ = geometry_msgs::PoseStamped();
|
||||
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
double oscillation_timeout_tmp;
|
||||
private_nh.param("oscillation_timeout", oscillation_timeout_tmp, 0.0);
|
||||
ros::Duration oscillation_timeout(oscillation_timeout_tmp);
|
||||
|
||||
double oscillation_distance;
|
||||
private_nh.param("oscillation_distance", oscillation_distance, 0.03);
|
||||
|
||||
mbf_msgs::ExePathResult result;
|
||||
mbf_msgs::ExePathFeedback feedback;
|
||||
|
||||
typename AbstractControllerExecution::ControllerState state_moving_input;
|
||||
bool controller_active = true;
|
||||
|
||||
goal_mtx_.lock();
|
||||
const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get());
|
||||
|
||||
const std::vector<geometry_msgs::PoseStamped> &plan = goal.path.poses;
|
||||
if (plan.empty())
|
||||
{
|
||||
fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan!", result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
|
||||
controller_active = false;
|
||||
goal_mtx_.unlock();
|
||||
return;
|
||||
}
|
||||
|
||||
goal_pose_ = plan.back();
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Called action \""
|
||||
<< name_ << "\" with plan:" << std::endl
|
||||
<< "frame: \"" << goal.path.header.frame_id << "\" " << std::endl
|
||||
<< "stamp: " << goal.path.header.stamp << std::endl
|
||||
<< "poses: " << goal.path.poses.size() << std::endl
|
||||
<< "goal: (" << goal_pose_.pose.position.x << ", "
|
||||
<< goal_pose_.pose.position.y << ", "
|
||||
<< goal_pose_.pose.position.z << ")");
|
||||
|
||||
goal_mtx_.unlock();
|
||||
|
||||
|
||||
geometry_msgs::PoseStamped oscillation_pose;
|
||||
ros::Time last_oscillation_reset = ros::Time::now();
|
||||
|
||||
bool first_cycle = true;
|
||||
|
||||
while (controller_active && ros::ok())
|
||||
{
|
||||
// goal_handle could change between the loop cycles due to adapting the plan
|
||||
// with a new goal received for the same concurrency slot
|
||||
if (!robot_info_.getRobotPose(robot_pose_))
|
||||
{
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::TF_ERROR, "Could not get the robot pose!", result);
|
||||
goal_mtx_.lock();
|
||||
goal_handle.setAborted(result, result.message);
|
||||
goal_mtx_.unlock();
|
||||
ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
|
||||
break;
|
||||
}
|
||||
|
||||
if (first_cycle)
|
||||
{
|
||||
// init oscillation pose
|
||||
oscillation_pose = robot_pose_;
|
||||
}
|
||||
|
||||
goal_mtx_.lock();
|
||||
state_moving_input = execution.getState();
|
||||
|
||||
switch (state_moving_input)
|
||||
{
|
||||
case AbstractControllerExecution::INITIALIZED:
|
||||
execution.setNewPlan(plan, goal.tolerance_from_action, goal.dist_tolerance, goal.angle_tolerance);
|
||||
execution.start();
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::STOPPED:
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller has been stopped rigorously!");
|
||||
controller_active = false;
|
||||
result.outcome = mbf_msgs::ExePathResult::STOPPED;
|
||||
result.message = "Controller has been stopped!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::CANCELED:
|
||||
ROS_INFO_STREAM("Action \"exe_path\" canceled");
|
||||
fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Controller canceled", result);
|
||||
goal_handle.setCanceled(result, result.message);
|
||||
controller_active = false;
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::STARTED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "The moving has been started!");
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::PLANNING:
|
||||
if (execution.isPatienceExceeded())
|
||||
{
|
||||
ROS_INFO_STREAM("Try to cancel the plugin \"" << name_ << "\" after the patience time has been exceeded!");
|
||||
if (execution.cancel())
|
||||
{
|
||||
ROS_INFO_STREAM("Successfully canceled the plugin \"" << name_ << "\" after the patience time has been exceeded!");
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::MAX_RETRIES:
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!");
|
||||
controller_active = false;
|
||||
fillExePathResult(execution.getOutcome(), execution.getMessage(), result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::PAT_EXCEEDED:
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the patience time");
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::PAT_EXCEEDED, execution.getMessage(), result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::NO_PLAN:
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller has been started without a plan!");
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started without a path", result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::EMPTY_PLAN:
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller has received an empty plan");
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan", result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::INVALID_PLAN:
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller has received an invalid plan");
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an invalid plan", result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::NO_LOCAL_CMD:
|
||||
ROS_WARN_STREAM_THROTTLE_NAMED(3, name_, "No velocity command received from controller! "
|
||||
<< execution.getMessage());
|
||||
publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd());
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::GOT_LOCAL_CMD:
|
||||
if (!oscillation_timeout.isZero())
|
||||
{
|
||||
// check if oscillating
|
||||
if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance)
|
||||
{
|
||||
last_oscillation_reset = ros::Time::now();
|
||||
oscillation_pose = robot_pose_;
|
||||
}
|
||||
else if (last_oscillation_reset + oscillation_timeout < ros::Time::now())
|
||||
{
|
||||
ROS_WARN_STREAM_NAMED(name_, "The controller is oscillating for "
|
||||
<< (ros::Time::now() - last_oscillation_reset).toSec() << "s");
|
||||
|
||||
execution.cancel();
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::OSCILLATION, "Oscillation detected!", result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
}
|
||||
}
|
||||
publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd());
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::ARRIVED_GOAL:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal");
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::SUCCESS, "Controller succeeded; arrived at goal!", result);
|
||||
goal_handle.setSucceeded(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractControllerExecution::INTERNAL_ERROR:
|
||||
ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin: " << execution.getMessage());
|
||||
controller_active = false;
|
||||
fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
default:
|
||||
std::stringstream ss;
|
||||
ss << "Internal error: Unknown state in a move base flex controller execution with the number: "
|
||||
<< static_cast<int>(state_moving_input);
|
||||
fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, ss.str(), result);
|
||||
ROS_FATAL_STREAM_NAMED(name_, result.message);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
controller_active = false;
|
||||
}
|
||||
goal_mtx_.unlock();
|
||||
|
||||
if (controller_active)
|
||||
{
|
||||
// try to sleep a bit
|
||||
// normally this thread should be woken up from the controller execution thread
|
||||
// in order to transfer the results to the controller
|
||||
execution.waitForStateUpdate(boost::chrono::milliseconds(500));
|
||||
}
|
||||
|
||||
first_cycle = false;
|
||||
} // while (controller_active && ros::ok())
|
||||
|
||||
if (!controller_active)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
|
||||
}
|
||||
else
|
||||
{
|
||||
// normal on continuous replanning
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
|
||||
}
|
||||
}
|
||||
|
||||
void ControllerAction::publishExePathFeedback(
|
||||
GoalHandle &goal_handle,
|
||||
uint32_t outcome, const std::string &message,
|
||||
const geometry_msgs::TwistStamped ¤t_twist)
|
||||
{
|
||||
mbf_msgs::ExePathFeedback feedback;
|
||||
feedback.outcome = outcome;
|
||||
feedback.message = message;
|
||||
|
||||
feedback.last_cmd_vel = current_twist;
|
||||
if (feedback.last_cmd_vel.header.stamp.isZero())
|
||||
feedback.last_cmd_vel.header.stamp = ros::Time::now();
|
||||
|
||||
feedback.current_pose = robot_pose_;
|
||||
feedback.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
|
||||
feedback.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
|
||||
goal_handle.publishFeedback(feedback);
|
||||
}
|
||||
|
||||
void ControllerAction::fillExePathResult(
|
||||
uint32_t outcome, const std::string &message,
|
||||
mbf_msgs::ExePathResult &result)
|
||||
{
|
||||
result.outcome = outcome;
|
||||
result.message = message;
|
||||
result.final_pose = robot_pose_;
|
||||
result.dist_to_goal = static_cast<float>(mbf_utility::distance(robot_pose_, goal_pose_));
|
||||
result.angle_to_goal = static_cast<float>(mbf_utility::angle(robot_pose_, goal_pose_));
|
||||
}
|
||||
|
||||
} /* mbf_abstract_nav */
|
||||
40
navigations/move_base_flex/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py
Executable file
40
navigations/move_base_flex/mbf_abstract_nav/src/mbf_abstract_nav/__init__.py
Executable file
@@ -0,0 +1,40 @@
|
||||
# Generic set of parameters required by any MBF-based navigation framework
|
||||
# To use:
|
||||
#
|
||||
# from mbf_abstract_nav import add_mbf_abstract_nav_params
|
||||
# gen = ParameterGenerator()
|
||||
# add_mbf_abstract_nav_params(gen)
|
||||
# ...
|
||||
# WARN: parameters added here must be copied on the specific MBF implementation reconfigure callback, e.g. in:
|
||||
# https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp#L130
|
||||
# Also, you need to touch https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/cfg/MoveBaseFlex.cfg
|
||||
# when recompiling to ensure configuration code is regenerated with the new parameters
|
||||
|
||||
# need this only for dataype declarations
|
||||
from dynamic_reconfigure.parameter_generator_catkin import str_t, double_t, int_t, bool_t
|
||||
|
||||
|
||||
def add_mbf_abstract_nav_params(gen):
|
||||
gen.add("planner_frequency", double_t, 0,
|
||||
"The rate in Hz at which to run the planning loop", 0, 0, 100)
|
||||
gen.add("planner_patience", double_t, 0,
|
||||
"How long the planner will wait in seconds in an attempt to find a valid plan before giving up", 5.0, 0, 100)
|
||||
gen.add("planner_max_retries", int_t, 0,
|
||||
"How many times we will recall the planner in an attempt to find a valid plan before giving up", -1, -1, 1000)
|
||||
|
||||
gen.add("controller_frequency", double_t, 0,
|
||||
"The rate in Hz at which to run the control loop and send velocity commands to the base", 20, 0, 100)
|
||||
gen.add("controller_patience", double_t, 0,
|
||||
"How long the controller will wait in seconds without receiving a valid control before giving up", 5.0, 0, 100)
|
||||
gen.add("controller_max_retries", int_t, 0,
|
||||
"How many times we will recall the controller in an attempt to find a valid command before giving up", -1, -1, 1000)
|
||||
|
||||
gen.add("recovery_enabled", bool_t, 0,
|
||||
"Whether or not to enable the move_base_flex recovery behaviors to attempt to clear out space", True)
|
||||
gen.add("recovery_patience", double_t, 0,
|
||||
"How much time we allow recovery behaviors to complete before canceling (or stopping if cancel fails) them", 15.0, 0, 100)
|
||||
|
||||
gen.add("oscillation_timeout", double_t, 0,
|
||||
"How long in seconds to allow for oscillation before executing recovery behaviors", 0.0, 0, 60)
|
||||
gen.add("oscillation_distance", double_t, 0,
|
||||
"How far in meters the robot must move to be considered not to be oscillating", 0.5, 0, 10)
|
||||
586
navigations/move_base_flex/mbf_abstract_nav/src/move_base_action.cpp
Executable file
586
navigations/move_base_flex/mbf_abstract_nav/src/move_base_action.cpp
Executable file
@@ -0,0 +1,586 @@
|
||||
/*
|
||||
* 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_action.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <mbf_utility/navigation_utility.h>
|
||||
|
||||
#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
|
||||
#include "mbf_abstract_nav/move_base_action.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
MoveBaseAction::MoveBaseAction(const std::string &name,
|
||||
const mbf_utility::RobotInformation &robot_info,
|
||||
const std::vector<std::string> &behaviors)
|
||||
: name_(name), robot_info_(robot_info), private_nh_("~"),
|
||||
action_client_exe_path_(private_nh_, "exe_path"),
|
||||
action_client_get_path_(private_nh_, "get_path"),
|
||||
action_client_recovery_(private_nh_, "recovery"),
|
||||
oscillation_timeout_(0),
|
||||
oscillation_distance_(0),
|
||||
recovery_enabled_(true),
|
||||
behaviors_(behaviors),
|
||||
action_state_(NONE),
|
||||
recovery_trigger_(NONE),
|
||||
replanning_(false),
|
||||
replanning_rate_(1.0)
|
||||
{
|
||||
}
|
||||
|
||||
MoveBaseAction::~MoveBaseAction()
|
||||
{
|
||||
}
|
||||
|
||||
void MoveBaseAction::reconfigure(
|
||||
mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
|
||||
{
|
||||
if (config.planner_frequency > 0.0)
|
||||
{
|
||||
boost::lock_guard<boost::mutex> guard(replanning_mtx_);
|
||||
if (!replanning_)
|
||||
{
|
||||
replanning_ = true;
|
||||
if (action_state_ == EXE_PATH &&
|
||||
action_client_get_path_.getState() != actionlib::SimpleClientGoalState::PENDING &&
|
||||
action_client_get_path_.getState() != actionlib::SimpleClientGoalState::ACTIVE)
|
||||
{
|
||||
// exe_path is running and user has enabled replanning
|
||||
ROS_INFO_STREAM_NAMED("move_base", "Planner frequency set to " << config.planner_frequency
|
||||
<< ": start replanning, using the \"get_path\" action!");
|
||||
action_client_get_path_.sendGoal(
|
||||
get_path_goal_,
|
||||
boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
|
||||
);
|
||||
}
|
||||
}
|
||||
replanning_rate_ = ros::Rate(config.planner_frequency);
|
||||
}
|
||||
else
|
||||
replanning_ = false;
|
||||
oscillation_timeout_ = ros::Duration(config.oscillation_timeout);
|
||||
oscillation_distance_ = config.oscillation_distance;
|
||||
recovery_enabled_ = config.recovery_enabled;
|
||||
}
|
||||
|
||||
void MoveBaseAction::cancel()
|
||||
{
|
||||
action_state_ = CANCELED;
|
||||
|
||||
if (!action_client_get_path_.getState().isDone())
|
||||
{
|
||||
action_client_get_path_.cancelGoal();
|
||||
}
|
||||
|
||||
if (!action_client_exe_path_.getState().isDone())
|
||||
{
|
||||
action_client_exe_path_.cancelGoal();
|
||||
}
|
||||
|
||||
if (!action_client_recovery_.getState().isDone())
|
||||
{
|
||||
action_client_recovery_.cancelGoal();
|
||||
}
|
||||
}
|
||||
|
||||
void MoveBaseAction::start(GoalHandle &goal_handle)
|
||||
{
|
||||
action_state_ = GET_PATH;
|
||||
|
||||
goal_handle.setAccepted();
|
||||
|
||||
goal_handle_ = goal_handle;
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Start action \"move_base\"");
|
||||
|
||||
const mbf_msgs::MoveBaseGoal& goal = *goal_handle.getGoal();
|
||||
|
||||
mbf_msgs::MoveBaseResult move_base_result;
|
||||
|
||||
get_path_goal_.target_pose = goal.target_pose;
|
||||
get_path_goal_.use_start_pose = false; // use the robot pose
|
||||
get_path_goal_.planner = goal.planner;
|
||||
get_path_goal_.tolerance_from_action = goal.tolerance_from_action;
|
||||
get_path_goal_.dist_tolerance = goal.dist_tolerance;
|
||||
get_path_goal_.angle_tolerance = goal.angle_tolerance;
|
||||
|
||||
exe_path_goal_.controller = goal.controller;
|
||||
exe_path_goal_.tolerance_from_action = goal.tolerance_from_action;
|
||||
exe_path_goal_.dist_tolerance = goal.dist_tolerance;
|
||||
exe_path_goal_.angle_tolerance = goal.angle_tolerance;
|
||||
|
||||
ros::Duration connection_timeout(1.0);
|
||||
|
||||
last_oscillation_reset_ = ros::Time::now();
|
||||
|
||||
// start recovering with the first behavior, use the recovery behaviors from the action request, if specified,
|
||||
// otherwise, use all loaded behaviors.
|
||||
|
||||
recovery_behaviors_ = goal.recovery_behaviors.empty() ? behaviors_ : goal.recovery_behaviors;
|
||||
current_recovery_behavior_ = recovery_behaviors_.begin();
|
||||
|
||||
// get the current robot pose only at the beginning, as exe_path will keep updating it as we move
|
||||
if (!robot_info_.getRobotPose(robot_pose_))
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!");
|
||||
move_base_result.message = "Could not get the current robot pose!";
|
||||
move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
|
||||
goal_handle.setAborted(move_base_result, move_base_result.message);
|
||||
return;
|
||||
}
|
||||
goal_pose_ = goal.target_pose;
|
||||
|
||||
// wait for server connections
|
||||
if (!action_client_get_path_.waitForServer(connection_timeout) ||
|
||||
!action_client_exe_path_.waitForServer(connection_timeout) ||
|
||||
!action_client_recovery_.waitForServer(connection_timeout))
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions: "
|
||||
"\"get_path\", \"exe_path\", \"recovery \"!");
|
||||
move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
|
||||
move_base_result.message = "Could not connect to the move_base_flex actions!";
|
||||
goal_handle.setAborted(move_base_result, move_base_result.message);
|
||||
return;
|
||||
}
|
||||
|
||||
// call get_path action server to get a first plan
|
||||
action_client_get_path_.sendGoal(
|
||||
get_path_goal_,
|
||||
boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2));
|
||||
}
|
||||
|
||||
void MoveBaseAction::actionExePathActive()
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active.");
|
||||
}
|
||||
|
||||
void MoveBaseAction::actionExePathFeedback(
|
||||
const mbf_msgs::ExePathFeedbackConstPtr &feedback)
|
||||
{
|
||||
mbf_msgs::MoveBaseFeedback move_base_feedback;
|
||||
move_base_feedback.outcome = feedback->outcome;
|
||||
move_base_feedback.message = feedback->message;
|
||||
move_base_feedback.angle_to_goal = feedback->angle_to_goal;
|
||||
move_base_feedback.dist_to_goal = feedback->dist_to_goal;
|
||||
move_base_feedback.current_pose = feedback->current_pose;
|
||||
move_base_feedback.last_cmd_vel = feedback->last_cmd_vel;
|
||||
robot_pose_ = feedback->current_pose;
|
||||
goal_handle_.publishFeedback(move_base_feedback);
|
||||
|
||||
// we create a navigation-level oscillation detection using exe_path action's feedback,
|
||||
// as the later doesn't handle oscillations created by quickly failing repeated plans
|
||||
|
||||
// if oscillation detection is enabled by oscillation_timeout != 0
|
||||
if (!oscillation_timeout_.isZero())
|
||||
{
|
||||
// check if oscillating
|
||||
// moved more than the minimum oscillation distance
|
||||
if (mbf_utility::distance(robot_pose_, last_oscillation_pose_) >= oscillation_distance_)
|
||||
{
|
||||
last_oscillation_reset_ = ros::Time::now();
|
||||
last_oscillation_pose_ = robot_pose_;
|
||||
|
||||
if (recovery_trigger_ == OSCILLATING)
|
||||
{
|
||||
ROS_INFO_NAMED("move_base", "Recovered from robot oscillation: restart recovery behaviors");
|
||||
current_recovery_behavior_ = recovery_behaviors_.begin();
|
||||
recovery_trigger_ = NONE;
|
||||
}
|
||||
}
|
||||
else if (last_oscillation_reset_ + oscillation_timeout_ < ros::Time::now())
|
||||
{
|
||||
std::stringstream oscillation_msgs;
|
||||
oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!";
|
||||
ROS_WARN_STREAM_NAMED("move_base", oscillation_msgs.str());
|
||||
action_client_exe_path_.cancelGoal();
|
||||
|
||||
if (attemptRecovery())
|
||||
{
|
||||
recovery_trigger_ = OSCILLATING;
|
||||
}
|
||||
else
|
||||
{
|
||||
mbf_msgs::MoveBaseResult move_base_result;
|
||||
move_base_result.outcome = mbf_msgs::MoveBaseResult::OSCILLATION;
|
||||
move_base_result.message = oscillation_msgs.str();
|
||||
move_base_result.final_pose = robot_pose_;
|
||||
move_base_result.angle_to_goal = move_base_feedback.angle_to_goal;
|
||||
move_base_result.dist_to_goal = move_base_feedback.dist_to_goal;
|
||||
goal_handle_.setAborted(move_base_result, move_base_result.message);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MoveBaseAction::actionGetPathDone(
|
||||
const actionlib::SimpleClientGoalState &state,
|
||||
const mbf_msgs::GetPathResultConstPtr &result_ptr)
|
||||
{
|
||||
const mbf_msgs::GetPathResult &get_path_result = *result_ptr;
|
||||
mbf_msgs::MoveBaseResult move_base_result;
|
||||
|
||||
// copy result from get_path action
|
||||
fillMoveBaseResult(get_path_result, move_base_result);
|
||||
|
||||
switch (state.state_)
|
||||
{
|
||||
case actionlib::SimpleClientGoalState::PENDING:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "get_path PENDING state not implemented, this should not be reachable!");
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::SUCCEEDED:
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
|
||||
<< "move_base\" received a path from \""
|
||||
<< "get_path\": " << state.getText());
|
||||
|
||||
exe_path_goal_.path = get_path_result.path;
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Action \""
|
||||
<< "move_base\" sends the path to \""
|
||||
<< "exe_path\".");
|
||||
|
||||
if (recovery_trigger_ == GET_PATH)
|
||||
{
|
||||
ROS_WARN_NAMED("move_base", "Recovered from planner failure: restart recovery behaviors");
|
||||
current_recovery_behavior_ = recovery_behaviors_.begin();
|
||||
recovery_trigger_ = NONE;
|
||||
}
|
||||
|
||||
action_client_exe_path_.sendGoal(
|
||||
exe_path_goal_,
|
||||
boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
|
||||
boost::bind(&MoveBaseAction::actionExePathActive, this),
|
||||
boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
|
||||
action_state_ = EXE_PATH;
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::ABORTED:
|
||||
|
||||
if (attemptRecovery())
|
||||
{
|
||||
recovery_trigger_ = GET_PATH;
|
||||
}
|
||||
else
|
||||
{
|
||||
// copy result from get_path action
|
||||
ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the planner: " << get_path_result.message);
|
||||
goal_handle_.setAborted(move_base_result, state.getText());
|
||||
}
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::RECALLED:
|
||||
case actionlib::SimpleClientGoalState::PREEMPTED:
|
||||
ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
|
||||
if (action_state_ == CANCELED)
|
||||
{
|
||||
// move_base preempted while executing get_path; fill result and report canceled to the client
|
||||
ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing get_path");
|
||||
goal_handle_.setCanceled(move_base_result, state.getText());
|
||||
}
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::REJECTED:
|
||||
ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"get_path\" has been " << state.toString());
|
||||
goal_handle_.setCanceled(move_base_result, state.getText());
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::LOST:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"get_path\"!");
|
||||
goal_handle_.setAborted();
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
|
||||
default:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "Reached unknown action server state!");
|
||||
goal_handle_.setAborted();
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
}
|
||||
|
||||
// start replanning if enabled (can be disabled by dynamic reconfigure) and if we started following a path
|
||||
if (!replanning_ || action_state_ != EXE_PATH)
|
||||
return;
|
||||
|
||||
// we reset the replan clock (we can have been stopped for a while) and make a fist sleep, so we don't replan
|
||||
// just after start moving
|
||||
boost::lock_guard<boost::mutex> guard(replanning_mtx_);
|
||||
replanning_rate_.reset();
|
||||
replanning_rate_.sleep();
|
||||
if (!replanning_ || action_state_ != EXE_PATH ||
|
||||
action_client_get_path_.getState() == actionlib::SimpleClientGoalState::PENDING ||
|
||||
action_client_get_path_.getState() == actionlib::SimpleClientGoalState::ACTIVE)
|
||||
return; // another chance to stop replannings after waiting for replanning period
|
||||
ROS_INFO_STREAM_NAMED("move_base", "Start replanning, using the \"get_path\" action!");
|
||||
action_client_get_path_.sendGoal(
|
||||
get_path_goal_,
|
||||
boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)
|
||||
);
|
||||
}
|
||||
|
||||
void MoveBaseAction::actionExePathDone(
|
||||
const actionlib::SimpleClientGoalState &state,
|
||||
const mbf_msgs::ExePathResultConstPtr &result_ptr)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished.");
|
||||
|
||||
const mbf_msgs::ExePathResult& exe_path_result = *result_ptr;
|
||||
mbf_msgs::MoveBaseResult move_base_result;
|
||||
|
||||
// copy result from exe_path action
|
||||
fillMoveBaseResult(exe_path_result, move_base_result);
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Current state:" << state.toString());
|
||||
|
||||
switch (state.state_)
|
||||
{
|
||||
case actionlib::SimpleClientGoalState::SUCCEEDED:
|
||||
move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
|
||||
move_base_result.message = "Action \"move_base\" succeeded!";
|
||||
ROS_INFO_STREAM_NAMED("move_base", move_base_result.message);
|
||||
goal_handle_.setSucceeded(move_base_result, move_base_result.message);
|
||||
action_state_ = SUCCEEDED;
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::ABORTED:
|
||||
action_state_ = FAILED;
|
||||
|
||||
switch (exe_path_result.outcome)
|
||||
{
|
||||
case mbf_msgs::ExePathResult::INVALID_PATH:
|
||||
case mbf_msgs::ExePathResult::TF_ERROR:
|
||||
case mbf_msgs::ExePathResult::NOT_INITIALIZED:
|
||||
case mbf_msgs::ExePathResult::INVALID_PLUGIN:
|
||||
case mbf_msgs::ExePathResult::INTERNAL_ERROR:
|
||||
// none of these errors is recoverable
|
||||
goal_handle_.setAborted(move_base_result, state.getText());
|
||||
break;
|
||||
|
||||
default:
|
||||
// all the rest are, so we start calling the recovery behaviors in sequence
|
||||
|
||||
if (attemptRecovery())
|
||||
{
|
||||
recovery_trigger_ = EXE_PATH;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << exe_path_result.message);
|
||||
goal_handle_.setAborted(move_base_result, state.getText());
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::RECALLED:
|
||||
case actionlib::SimpleClientGoalState::PREEMPTED:
|
||||
ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
|
||||
if (action_state_ == CANCELED)
|
||||
{
|
||||
// move_base preempted while executing exe_path; fill result and report canceled to the client
|
||||
ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing exe_path");
|
||||
goal_handle_.setCanceled(move_base_result, state.getText());
|
||||
}
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::REJECTED:
|
||||
ROS_ERROR_STREAM_NAMED("move_base", "The last action goal to \"exe_path\" has been " << state.toString());
|
||||
goal_handle_.setCanceled(move_base_result, state.getText());
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::LOST:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"exe_path\"!");
|
||||
goal_handle_.setAborted();
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
|
||||
default:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown SimpleActionServer state!");
|
||||
goal_handle_.setAborted();
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool MoveBaseAction::attemptRecovery()
|
||||
{
|
||||
if (!recovery_enabled_)
|
||||
{
|
||||
ROS_WARN_STREAM_NAMED("move_base", "Recovery behaviors are disabled!");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (current_recovery_behavior_ == recovery_behaviors_.end())
|
||||
{
|
||||
if (recovery_behaviors_.empty())
|
||||
{
|
||||
ROS_WARN_STREAM_NAMED("move_base", "No Recovery Behaviors loaded!");
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN_STREAM_NAMED("move_base", "Executed all available recovery behaviors!");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
recovery_goal_.behavior = *current_recovery_behavior_;
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Start recovery behavior\""
|
||||
<< *current_recovery_behavior_ <<"\".");
|
||||
action_client_recovery_.sendGoal(
|
||||
recovery_goal_,
|
||||
boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
|
||||
);
|
||||
action_state_ = RECOVERY;
|
||||
return true;
|
||||
}
|
||||
|
||||
void MoveBaseAction::actionRecoveryDone(
|
||||
const actionlib::SimpleClientGoalState &state,
|
||||
const mbf_msgs::RecoveryResultConstPtr &result_ptr)
|
||||
{
|
||||
// give the robot some time to stop oscillating after executing the recovery behavior
|
||||
last_oscillation_reset_ = ros::Time::now();
|
||||
|
||||
const mbf_msgs::RecoveryResult& recovery_result = *result_ptr;
|
||||
mbf_msgs::MoveBaseResult move_base_result;
|
||||
|
||||
// copy result from recovery action
|
||||
fillMoveBaseResult(recovery_result, move_base_result);
|
||||
|
||||
switch (state.state_)
|
||||
{
|
||||
case actionlib::SimpleClientGoalState::REJECTED:
|
||||
case actionlib::SimpleClientGoalState::ABORTED:
|
||||
action_state_ = FAILED;
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "The recovery behavior \""
|
||||
<< *current_recovery_behavior_ << "\" has failed. ");
|
||||
ROS_DEBUG_STREAM("Recovery behavior message: " << recovery_result.message
|
||||
<< ", outcome: " << recovery_result.outcome);
|
||||
|
||||
current_recovery_behavior_++; // use next behavior;
|
||||
if (current_recovery_behavior_ == recovery_behaviors_.end())
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED("move_base",
|
||||
"All recovery behaviors failed. Abort recovering and abort the move_base action");
|
||||
goal_handle_.setAborted(move_base_result, "All recovery behaviors failed.");
|
||||
}
|
||||
else
|
||||
{
|
||||
recovery_goal_.behavior = *current_recovery_behavior_;
|
||||
|
||||
ROS_INFO_STREAM_NAMED("move_base", "Run the next recovery behavior \""
|
||||
<< *current_recovery_behavior_ << "\".");
|
||||
action_client_recovery_.sendGoal(
|
||||
recovery_goal_,
|
||||
boost::bind(&MoveBaseAction::actionRecoveryDone, this, _1, _2)
|
||||
);
|
||||
}
|
||||
break;
|
||||
case actionlib::SimpleClientGoalState::SUCCEEDED:
|
||||
//go to planning state
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Execution of the recovery behavior \""
|
||||
<< *current_recovery_behavior_ << "\" succeeded!");
|
||||
ROS_DEBUG_STREAM_NAMED("move_base",
|
||||
"Try planning again and increment the current recovery behavior in the list.");
|
||||
action_state_ = GET_PATH;
|
||||
current_recovery_behavior_++; // use next behavior, the next time;
|
||||
action_client_get_path_.sendGoal(
|
||||
get_path_goal_,
|
||||
boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)
|
||||
);
|
||||
break;
|
||||
case actionlib::SimpleClientGoalState::RECALLED:
|
||||
case actionlib::SimpleClientGoalState::PREEMPTED:
|
||||
ROS_INFO_STREAM_NAMED("move_base", "The last action goal to \"recovery\" has been preempted!");
|
||||
if (action_state_ == CANCELED)
|
||||
{
|
||||
// move_base preempted while executing a recovery; fill result and report canceled to the client
|
||||
ROS_INFO_STREAM_NAMED("move_base", "move_base preempted while executing a recovery behavior");
|
||||
goal_handle_.setCanceled(move_base_result, state.getText());
|
||||
}
|
||||
break;
|
||||
|
||||
case actionlib::SimpleClientGoalState::LOST:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "Connection lost to the action \"recovery\"!");
|
||||
goal_handle_.setAborted();
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
default:
|
||||
ROS_FATAL_STREAM_NAMED("move_base", "Reached unreachable case! Unknown state!");
|
||||
goal_handle_.setAborted();
|
||||
action_state_ = FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MoveBaseAction::actionGetPathReplanningDone(
|
||||
const actionlib::SimpleClientGoalState &state,
|
||||
const mbf_msgs::GetPathResultConstPtr &result)
|
||||
{
|
||||
if (!replanning_ || action_state_ != EXE_PATH)
|
||||
return; // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure)
|
||||
|
||||
if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
|
||||
exe_path_goal_.path = result->path;
|
||||
mbf_msgs::ExePathGoal goal(exe_path_goal_);
|
||||
action_client_exe_path_.sendGoal(
|
||||
goal,
|
||||
boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2),
|
||||
boost::bind(&MoveBaseAction::actionExePathActive, this),
|
||||
boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1));
|
||||
}
|
||||
|
||||
replanning_mtx_.lock();
|
||||
replanning_rate_.sleep();
|
||||
replanning_mtx_.unlock();
|
||||
|
||||
if (!replanning_ || action_state_ != EXE_PATH)
|
||||
return; // another chance to stop replannings after waiting for replanning period
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!");
|
||||
action_client_get_path_.sendGoal(
|
||||
get_path_goal_,
|
||||
boost::bind(&MoveBaseAction::actionGetPathReplanningDone, this, _1, _2)); // replanning
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
286
navigations/move_base_flex/mbf_abstract_nav/src/planner_action.cpp
Executable file
286
navigations/move_base_flex/mbf_abstract_nav/src/planner_action.cpp
Executable file
@@ -0,0 +1,286 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* planner_action.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <sstream>
|
||||
|
||||
#include "mbf_abstract_nav/planner_action.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
PlannerAction::PlannerAction(
|
||||
const std::string &name,
|
||||
const mbf_utility::RobotInformation &robot_info)
|
||||
: AbstractActionBase(name, robot_info, boost::bind(&mbf_abstract_nav::PlannerAction::run, this, _1, _2)), path_seq_count_(0)
|
||||
{
|
||||
ros::NodeHandle private_nh("~");
|
||||
// informative topics: current navigation goal
|
||||
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
|
||||
}
|
||||
|
||||
void PlannerAction::run(GoalHandle &goal_handle, AbstractPlannerExecution &execution)
|
||||
{
|
||||
const mbf_msgs::GetPathGoal& goal = *(goal_handle.getGoal().get());
|
||||
|
||||
mbf_msgs::GetPathResult result;
|
||||
geometry_msgs::PoseStamped start_pose;
|
||||
|
||||
result.path.header.seq = path_seq_count_++;
|
||||
result.path.header.frame_id = robot_info_.getGlobalFrame();
|
||||
|
||||
double dist_tolerance = goal.tolerance_from_action ? goal.dist_tolerance : 0.0;
|
||||
double angle_tolerance = goal.tolerance_from_action ? goal.angle_tolerance : 0.0;
|
||||
bool use_start_pose = goal.use_start_pose;
|
||||
current_goal_pub_.publish(goal.target_pose);
|
||||
|
||||
bool planner_active = true;
|
||||
|
||||
if (use_start_pose)
|
||||
{
|
||||
start_pose = goal.start_pose;
|
||||
const geometry_msgs::Point& p = start_pose.pose.position;
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Use the given start pose (" << p.x << ", " << p.y << ", " << p.z << ").");
|
||||
}
|
||||
else
|
||||
{
|
||||
// get the current robot pose
|
||||
if (!robot_info_.getRobotPose(start_pose))
|
||||
{
|
||||
result.outcome = mbf_msgs::GetPathResult::TF_ERROR;
|
||||
result.message = "Could not get the current robot pose!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
const geometry_msgs::Point& p = start_pose.pose.position;
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Got the current robot pose at ("
|
||||
<< p.x << ", " << p.y << ", " << p.z << ").");
|
||||
}
|
||||
}
|
||||
|
||||
AbstractPlannerExecution::PlanningState state_planning_input;
|
||||
|
||||
std::vector<geometry_msgs::PoseStamped> plan, global_plan;
|
||||
|
||||
while (planner_active && ros::ok())
|
||||
{
|
||||
// get the current state of the planning thread
|
||||
state_planning_input = execution.getState();
|
||||
|
||||
switch (state_planning_input)
|
||||
{
|
||||
case AbstractPlannerExecution::INITIALIZED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "planner state: initialized");
|
||||
if (!execution.start(start_pose, goal.target_pose, dist_tolerance, angle_tolerance))
|
||||
{
|
||||
result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
|
||||
result.message = "Another thread is still planning!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
|
||||
planner_active = false;
|
||||
}
|
||||
break;
|
||||
|
||||
case AbstractPlannerExecution::STARTED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "planner state: started");
|
||||
break;
|
||||
|
||||
case AbstractPlannerExecution::STOPPED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "planner state: stopped");
|
||||
ROS_WARN_STREAM_NAMED(name_, "Planning has been stopped rigorously!");
|
||||
result.outcome = mbf_msgs::GetPathResult::STOPPED;
|
||||
result.message = "Global planner has been stopped!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
|
||||
case AbstractPlannerExecution::CANCELED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "planner state: canceled");
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Global planner has been canceled successfully");
|
||||
result.path.header.stamp = ros::Time::now();
|
||||
result.outcome = mbf_msgs::GetPathResult::CANCELED;
|
||||
result.message = "Global planner has been canceled!";
|
||||
goal_handle.setCanceled(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
|
||||
// in progress
|
||||
case AbstractPlannerExecution::PLANNING:
|
||||
if (execution.isPatienceExceeded())
|
||||
{
|
||||
ROS_INFO_STREAM_NAMED(name_, "Global planner patience has been exceeded! Cancel planning...");
|
||||
execution.cancel();
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_DEBUG_THROTTLE_NAMED(2.0, name_, "planner state: planning");
|
||||
}
|
||||
break;
|
||||
|
||||
// found a new plan
|
||||
case AbstractPlannerExecution::FOUND_PLAN:
|
||||
// set time stamp to now
|
||||
result.path.header.stamp = ros::Time::now();
|
||||
plan = execution.getPlan();
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "planner state: found plan with cost: " << execution.getCost());
|
||||
|
||||
if (!transformPlanToGlobalFrame(plan, global_plan))
|
||||
{
|
||||
result.outcome = mbf_msgs::GetPathResult::TF_ERROR;
|
||||
result.message = "Could not transform the plan to the global frame!";
|
||||
|
||||
ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call.");
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (global_plan.empty())
|
||||
{
|
||||
result.outcome = mbf_msgs::GetPathResult::EMPTY_PATH;
|
||||
result.message = "Global planner returned an empty path!";
|
||||
|
||||
ROS_ERROR_STREAM_NAMED(name_, result.message);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
}
|
||||
|
||||
result.path.poses = global_plan;
|
||||
result.cost = execution.getCost();
|
||||
result.outcome = execution.getOutcome();
|
||||
result.message = execution.getMessage();
|
||||
goal_handle.setSucceeded(result, result.message);
|
||||
|
||||
planner_active = false;
|
||||
break;
|
||||
|
||||
// no plan found
|
||||
case AbstractPlannerExecution::NO_PLAN_FOUND:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "planner state: no plan found");
|
||||
result.outcome = execution.getOutcome();
|
||||
result.message = execution.getMessage();
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
|
||||
case AbstractPlannerExecution::MAX_RETRIES:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Global planner reached the maximum number of retries");
|
||||
result.outcome = execution.getOutcome();
|
||||
result.message = execution.getMessage();
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
|
||||
case AbstractPlannerExecution::PAT_EXCEEDED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Global planner exceeded the patience time");
|
||||
result.outcome = mbf_msgs::GetPathResult::PAT_EXCEEDED;
|
||||
result.message = "Global planner exceeded the patience time";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
break;
|
||||
|
||||
case AbstractPlannerExecution::INTERNAL_ERROR:
|
||||
ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin!"); // TODO getMessage from planning
|
||||
planner_active = false;
|
||||
result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
|
||||
result.message = "Internal error: Unknown error thrown by the plugin!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
default:
|
||||
result.outcome = mbf_msgs::GetPathResult::INTERNAL_ERROR;
|
||||
std::ostringstream ss;
|
||||
ss << "Internal error: Unknown state in a move base flex planner execution with the number: "
|
||||
<< static_cast<int>(state_planning_input);
|
||||
result.message = ss.str();
|
||||
ROS_FATAL_STREAM_NAMED(name_, result.message);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
planner_active = false;
|
||||
}
|
||||
|
||||
|
||||
if (planner_active)
|
||||
{
|
||||
// try to sleep a bit
|
||||
// normally this thread should be woken up from the planner execution thread
|
||||
// in order to transfer the results to the controller.
|
||||
boost::mutex mutex;
|
||||
boost::unique_lock<boost::mutex> lock(mutex);
|
||||
execution.waitForStateUpdate(boost::chrono::milliseconds(500));
|
||||
}
|
||||
} // while (planner_active && ros::ok())
|
||||
|
||||
if (!planner_active)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
|
||||
}
|
||||
}
|
||||
|
||||
bool PlannerAction::transformPlanToGlobalFrame(
|
||||
std::vector<geometry_msgs::PoseStamped> &plan, std::vector<geometry_msgs::PoseStamped> &global_plan)
|
||||
{
|
||||
global_plan.clear();
|
||||
std::vector<geometry_msgs::PoseStamped>::iterator iter;
|
||||
bool tf_success = false;
|
||||
for (iter = plan.begin(); iter != plan.end(); ++iter)
|
||||
{
|
||||
geometry_msgs::PoseStamped global_pose;
|
||||
tf_success = mbf_utility::transformPose(robot_info_.getTransformListener(), robot_info_.getGlobalFrame(),
|
||||
robot_info_.getTfTimeout(), *iter, global_pose);
|
||||
if (!tf_success)
|
||||
{
|
||||
ROS_ERROR_STREAM("Can not transform pose from the \"" << iter->header.frame_id << "\" frame into the \""
|
||||
<< robot_info_.getGlobalFrame() << "\" frame !");
|
||||
return false;
|
||||
}
|
||||
global_plan.push_back(global_pose);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
156
navigations/move_base_flex/mbf_abstract_nav/src/recovery_action.cpp
Executable file
156
navigations/move_base_flex/mbf_abstract_nav/src/recovery_action.cpp
Executable file
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* 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.
|
||||
*
|
||||
* recovery_action.cpp
|
||||
*
|
||||
* authors:
|
||||
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
|
||||
* Jorge Santos Simón <santos@magazino.eu>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mbf_abstract_nav/recovery_action.h"
|
||||
|
||||
namespace mbf_abstract_nav
|
||||
{
|
||||
|
||||
RecoveryAction::RecoveryAction(const std::string &name, const mbf_utility::RobotInformation &robot_info)
|
||||
: AbstractActionBase(name, robot_info, boost::bind(&mbf_abstract_nav::RecoveryAction::run, this, _1, _2)){}
|
||||
|
||||
void RecoveryAction::run(GoalHandle &goal_handle, AbstractRecoveryExecution &execution)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Start action " << name_);
|
||||
|
||||
const mbf_msgs::RecoveryGoal &goal = *goal_handle.getGoal();
|
||||
mbf_msgs::RecoveryResult result;
|
||||
result.used_plugin = goal.behavior;
|
||||
bool recovery_active = true;
|
||||
|
||||
typename AbstractRecoveryExecution::RecoveryState state_recovery_input;
|
||||
|
||||
while (recovery_active && ros::ok())
|
||||
{
|
||||
state_recovery_input = execution.getState();
|
||||
switch (state_recovery_input)
|
||||
{
|
||||
case AbstractRecoveryExecution::INITIALIZED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior \"" << goal.behavior << "\" initialized.");
|
||||
execution.start();
|
||||
break;
|
||||
|
||||
case AbstractRecoveryExecution::STOPPED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior stopped rigorously");
|
||||
result.outcome = mbf_msgs::RecoveryResult::STOPPED;
|
||||
result.message = "Recovery has been stopped!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
recovery_active = false;
|
||||
break;
|
||||
|
||||
case AbstractRecoveryExecution::STARTED:
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "Recovery behavior \"" << goal.behavior << "\" was started");
|
||||
break;
|
||||
|
||||
case AbstractRecoveryExecution::RECOVERING:
|
||||
|
||||
if (execution.isPatienceExceeded())
|
||||
{
|
||||
ROS_INFO_STREAM("Recovery behavior \"" << goal.behavior << "\" patience exceeded! Cancel recovering...");
|
||||
execution.cancel();
|
||||
}
|
||||
|
||||
ROS_DEBUG_STREAM_THROTTLE_NAMED(3, name_, "Recovering with: " << goal.behavior);
|
||||
break;
|
||||
|
||||
case AbstractRecoveryExecution::CANCELED:
|
||||
// Recovery behavior supports cancel and it worked
|
||||
recovery_active = false; // stopping the action
|
||||
result.outcome = mbf_msgs::RecoveryResult::CANCELED;
|
||||
result.message = "Recovery behaviour \"" + goal.behavior + "\" canceled!";
|
||||
goal_handle.setCanceled(result, result.message);
|
||||
ROS_DEBUG_STREAM_NAMED(name_, result.message);
|
||||
break;
|
||||
|
||||
case AbstractRecoveryExecution::RECOVERY_DONE:
|
||||
recovery_active = false; // stopping the action
|
||||
result.outcome = execution.getOutcome();
|
||||
result.message = execution.getMessage();
|
||||
if (result.message.empty())
|
||||
{
|
||||
if (result.outcome < 10)
|
||||
result.message = "Recovery \"" + goal.behavior + "\" done";
|
||||
else
|
||||
result.message = "Recovery \"" + goal.behavior + "\" FAILED";
|
||||
}
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED(name_, result.message);
|
||||
goal_handle.setSucceeded(result, result.message);
|
||||
break;
|
||||
|
||||
case AbstractRecoveryExecution::INTERNAL_ERROR:
|
||||
ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin!"); // TODO getMessage from recovery
|
||||
recovery_active = false;
|
||||
result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
|
||||
result.message = "Internal error: Unknown error thrown by the plugin!";
|
||||
goal_handle.setAborted(result, result.message);
|
||||
break;
|
||||
|
||||
default:
|
||||
result.outcome = mbf_msgs::RecoveryResult::INTERNAL_ERROR;
|
||||
std::stringstream ss;
|
||||
ss << "Internal error: Unknown state in a move base flex recovery execution with the number: "
|
||||
<< static_cast<int>(state_recovery_input);
|
||||
result.message = ss.str();
|
||||
ROS_FATAL_STREAM_NAMED(name_, result.message);
|
||||
goal_handle.setAborted(result, result.message);
|
||||
recovery_active = false;
|
||||
}
|
||||
|
||||
if (recovery_active)
|
||||
{
|
||||
// try to sleep a bit
|
||||
// normally the thread should be woken up from the recovery unit
|
||||
// in order to transfer the results to the controller
|
||||
execution.waitForStateUpdate(boost::chrono::milliseconds(500));
|
||||
}
|
||||
} // while (recovery_active && ros::ok())
|
||||
|
||||
if (!recovery_active)
|
||||
{
|
||||
ROS_DEBUG_STREAM_NAMED(name_, "\"" << name_ << "\" action ended properly.");
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR_STREAM_NAMED(name_, "\"" << name_ << "\" action has been stopped!");
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace mbf_abstract_nav */
|
||||
Reference in New Issue
Block a user