Skip to content

Commit

Permalink
Create 2 topics for current goal: controller and planner (#337)
Browse files Browse the repository at this point in the history
  • Loading branch information
corot authored Dec 21, 2023
1 parent bc19f73 commit 3c7b7f1
Show file tree
Hide file tree
Showing 12 changed files with 57 additions and 88 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -85,15 +85,13 @@ namespace mbf_abstract_nav
* @param controller_ptr Pointer to the controller plugin
* @param robot_info Current robot state
* @param vel_pub Velocity publisher
* @param goal_pub Current goal publisher
* @param config Initial configuration for this execution
*/
AbstractControllerExecution(
const std::string &name,
const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
const mbf_utility::RobotInformation &robot_info,
const ros::Publisher &vel_pub,
const ros::Publisher &goal_pub,
const MoveBaseFlexConfig &config);

/**
Expand Down Expand Up @@ -330,9 +328,6 @@ namespace mbf_abstract_nav
//! publisher for the current velocity command
ros::Publisher vel_pub_;

//! publisher for the current goal
ros::Publisher current_goal_pub_;

//! the current controller state
AbstractControllerExecution::ControllerState state_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -342,9 +342,6 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
//! cmd_vel publisher for all controller execution objects
ros::Publisher vel_pub_;

//! current_goal publisher for all controller execution objects
ros::Publisher goal_pub_;

//! current robot state
mbf_utility::RobotInformation robot_info_;

Expand Down
40 changes: 15 additions & 25 deletions mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
Original file line number Diff line number Diff line change
Expand Up @@ -52,52 +52,42 @@
namespace mbf_abstract_nav
{

class ControllerAction :
public AbstractActionBase<mbf_msgs::ExePathAction, AbstractControllerExecution>
class ControllerAction : public AbstractActionBase<mbf_msgs::ExePathAction, AbstractControllerExecution>
{
public:

public:
typedef boost::shared_ptr<ControllerAction> Ptr;

ControllerAction(const std::string &name,
const mbf_utility::RobotInformation &robot_info);
ControllerAction(const std::string& name, const mbf_utility::RobotInformation& robot_info);

/**
* @brief Start controller action.
* Override abstract action version to allow updating current plan without stopping execution.
* @param goal_handle Reference to the goal handle received on action execution callback.
* @param execution_ptr Pointer to the execution descriptor.
*/
void start(
GoalHandle &goal_handle,
typename AbstractControllerExecution::Ptr execution_ptr
);
void start(GoalHandle& goal_handle, typename AbstractControllerExecution::Ptr execution_ptr);

void runImpl(GoalHandle &goal_handle, AbstractControllerExecution& execution);
void runImpl(GoalHandle& goal_handle, AbstractControllerExecution& execution);

protected:
void publishExePathFeedback(
GoalHandle &goal_handle,
uint32_t outcome, const std::string &message,
const geometry_msgs::TwistStamped &current_twist);
void publishExePathFeedback(GoalHandle& goal_handle, uint32_t outcome, const std::string& message,
const geometry_msgs::TwistStamped& current_twist);

/**
* @brief Utility method to fill the ExePath action result in a single line
* @param outcome ExePath action outcome
* @param message ExePath action message
* @param result The action result to fill
*/
void fillExePathResult(
uint32_t outcome, const std::string &message,
mbf_msgs::ExePathResult &result);
void fillExePathResult(uint32_t outcome, const std::string& message, mbf_msgs::ExePathResult& result);

boost::mutex goal_mtx_; ///< lock goal handle for updating it while running
geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
geometry_msgs::PoseStamped goal_pose_; ///< Current goal pose
boost::mutex goal_mtx_; ///< lock goal handle for updating it while running
geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
geometry_msgs::PoseStamped goal_pose_; ///< Current goal pose

//! Publish the current goal pose (the last pose of the path we are following)
ros::Publisher goal_pub_;
};
}


} // namespace mbf_abstract_nav

#endif //MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_
#endif // MBF_ABSTRACT_NAV__CONTROLLER_ACTION_H_
29 changes: 11 additions & 18 deletions mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,43 +49,36 @@
#include "mbf_abstract_nav/abstract_action_base.hpp"
#include "mbf_abstract_nav/abstract_planner_execution.h"


namespace mbf_abstract_nav{

namespace mbf_abstract_nav
{

class PlannerAction : public AbstractActionBase<mbf_msgs::GetPathAction, AbstractPlannerExecution>
{
public:

public:
typedef boost::shared_ptr<PlannerAction> Ptr;

PlannerAction(
const std::string &name,
const mbf_utility::RobotInformation &robot_info
);

void runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &execution);
PlannerAction(const std::string& name, const mbf_utility::RobotInformation& robot_info);

protected:
void runImpl(GoalHandle& goal_handle, AbstractPlannerExecution& execution);

protected:
/**
* @brief Transforms a plan to the global frame (global_frame_) coord system.
* @param plan Input plan to be transformed.
* @param global_plan Output plan, which is then transformed to the global frame.
* @return true, if the transformation succeeded, false otherwise
*/
bool transformPlanToGlobalFrame(const std::vector<geometry_msgs::PoseStamped>& plan,
std::vector<geometry_msgs::PoseStamped>& global_plan);

private:
bool transformPlanToGlobalFrame(const std::vector<geometry_msgs::PoseStamped>& plan,
std::vector<geometry_msgs::PoseStamped>& global_plan);

private:
//! Publisher to publish the current goal pose, which is used for path planning
ros::Publisher current_goal_pub_;
ros::Publisher goal_pub_;

//! Path sequence counter
unsigned int path_seq_count_;
};

} /* mbf_abstract_nav */
} // namespace mbf_abstract_nav

#endif /* MBF_ABSTRACT_NAV__PLANNER_ACTION_H_ */
22 changes: 10 additions & 12 deletions mbf_abstract_nav/src/abstract_controller_execution.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,18 +48,17 @@ 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 mbf_utility::RobotInformation &robot_info,
const ros::Publisher &vel_pub,
const ros::Publisher &goal_pub,
const MoveBaseFlexConfig &config) :
AbstractExecutionBase(name, robot_info),
controller_(controller_ptr),
state_(INITIALIZED), moving_(false), max_retries_(0), patience_(0), vel_pub_(vel_pub), current_goal_pub_(goal_pub),
loop_rate_(DEFAULT_CONTROLLER_FREQUENCY)
const std::string& name, const mbf_abstract_core::AbstractController::Ptr& controller_ptr,
const mbf_utility::RobotInformation& robot_info, const ros::Publisher& vel_pub, const MoveBaseFlexConfig& config)
: AbstractExecutionBase(name, robot_info)
, controller_(controller_ptr)
, state_(INITIALIZED)
, moving_(false)
, max_retries_(0)
, patience_(0)
, vel_pub_(vel_pub)
, loop_rate_(DEFAULT_CONTROLLER_FREQUENCY)
{
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");

// non-dynamically reconfigurable parameters
Expand Down Expand Up @@ -375,7 +374,6 @@ void AbstractControllerExecution::run()
condition_.notify_all();
return;
}
current_goal_pub_.publish(plan.back());
}

// compute robot pose and store it in robot_pose_
Expand Down
8 changes: 2 additions & 6 deletions mbf_abstract_nav/src/abstract_navigation_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,12 +66,8 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
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);
vel_pub_ = ros::NodeHandle().advertise<geometry_msgs::Twist>("cmd_vel", 1);

action_server_get_path_ptr_ = ActionServerGetPathPtr(
new ActionServerGetPath(
Expand Down Expand Up @@ -323,7 +319,7 @@ mbf_abstract_nav::AbstractControllerExecution::Ptr AbstractNavigationServer::new
const mbf_abstract_core::AbstractController::Ptr &plugin_ptr)
{
return boost::make_shared<mbf_abstract_nav::AbstractControllerExecution>(plugin_name, plugin_ptr, robot_info_,
vel_pub_, goal_pub_, last_config_);
vel_pub_, last_config_);
}

mbf_abstract_nav::AbstractRecoveryExecution::Ptr AbstractNavigationServer::newRecoveryExecution(
Expand Down
5 changes: 5 additions & 0 deletions mbf_abstract_nav/src/controller_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,9 @@ ControllerAction::ControllerAction(
const mbf_utility::RobotInformation &robot_info)
: AbstractActionBase(action_name, robot_info)
{
// informative topics: current navigation goal
ros::NodeHandle private_nh("~");
goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("controller_goal", 1);
}

void ControllerAction::start(
Expand Down Expand Up @@ -85,6 +88,7 @@ void ControllerAction::start(
goal_handle.getGoal()->angle_tolerance);
// Update also goal pose, so the feedback remains consistent
goal_pose_ = goal_handle.getGoal()->path.poses.back();
goal_pub_.publish(goal_pose_);
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);
Expand Down Expand Up @@ -146,6 +150,7 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
}

goal_pose_ = plan.back();
goal_pub_.publish(goal_pose_);
ROS_DEBUG_STREAM_NAMED(name_, "Called action \""
<< name_ << "\" with plan:" << std::endl
<< "frame: \"" << goal.path.header.frame_id << "\" " << std::endl
Expand Down
6 changes: 3 additions & 3 deletions mbf_abstract_nav/src/planner_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,9 @@ PlannerAction::PlannerAction(
const mbf_utility::RobotInformation &robot_info)
: AbstractActionBase(name, robot_info), 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);
ros::NodeHandle private_nh("~");
goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("planner_goal", 1);
}

void PlannerAction::runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &execution)
Expand All @@ -67,7 +67,7 @@ void PlannerAction::runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &e

double tolerance = goal.tolerance;
bool use_start_pose = goal.use_start_pose;
current_goal_pub_.publish(goal.target_pose);
goal_pub_.publish(goal.target_pose);

bool planner_active = true;

Expand Down
5 changes: 2 additions & 3 deletions mbf_abstract_nav/test/abstract_controller_execution.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ struct AbstractControllerMock : public AbstractController
MOCK_METHOD0(cancel, bool());
};

ros::Publisher VEL_PUB, GOAL_PUB;
ros::Publisher VEL_PUB;
TFPtr TF_PTR;
mbf_utility::RobotInformation::Ptr ROBOT_INFO_PTR;

Expand All @@ -50,7 +50,7 @@ struct AbstractControllerExecutionFixture : public Test, public AbstractControll
{
AbstractControllerExecutionFixture()
: AbstractControllerExecution("a name", AbstractController::Ptr(new AbstractControllerMock()), *ROBOT_INFO_PTR,
VEL_PUB, GOAL_PUB, MoveBaseFlexConfig{})
VEL_PUB, MoveBaseFlexConfig{})
{
}

Expand Down Expand Up @@ -298,7 +298,6 @@ int main(int argc, char** argv)
ros::NodeHandle nh;
// setup the pubs as global objects
VEL_PUB = nh.advertise<Twist>("vel", 1);
GOAL_PUB = nh.advertise<PoseStamped>("pose", 1);

// setup the tf-publisher and robot info as a global objects
TF_PTR.reset(new TF());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,6 @@ class CostmapControllerExecution : public mbf_abstract_nav::AbstractControllerEx
* @param controller_ptr Shared pointer to the plugin to use.
* @param robot_info Current robot state
* @param vel_pub Velocity commands publisher.
* @param goal_pub Goal pose publisher (just vor visualization).
* @param costmap_ptr Shared pointer to the local costmap.
* @param config Current server configuration (dynamic).
*/
Expand All @@ -76,7 +75,6 @@ class CostmapControllerExecution : public mbf_abstract_nav::AbstractControllerEx
const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
const mbf_utility::RobotInformation &robot_info,
const ros::Publisher &vel_pub,
const ros::Publisher &goal_pub,
const CostmapWrapper::Ptr &costmap_ptr,
const MoveBaseFlexConfig &config);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,16 +42,14 @@
namespace mbf_costmap_nav
{

CostmapControllerExecution::CostmapControllerExecution(
const std::string &controller_name,
const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
const mbf_utility::RobotInformation &robot_info,
const ros::Publisher &vel_pub,
const ros::Publisher &goal_pub,
const CostmapWrapper::Ptr &costmap_ptr,
const MoveBaseFlexConfig &config)
: AbstractControllerExecution(controller_name, controller_ptr, robot_info, vel_pub, goal_pub, toAbstract(config)),
costmap_ptr_(costmap_ptr)
CostmapControllerExecution::CostmapControllerExecution(const std::string& controller_name,
const mbf_costmap_core::CostmapController::Ptr& controller_ptr,
const mbf_utility::RobotInformation& robot_info,
const ros::Publisher& vel_pub,
const CostmapWrapper::Ptr& costmap_ptr,
const MoveBaseFlexConfig& config)
: AbstractControllerExecution(controller_name, controller_ptr, robot_info, vel_pub, toAbstract(config))
, costmap_ptr_(costmap_ptr)
{
ros::NodeHandle private_nh("~");
private_nh.param("controller_lock_costmap", lock_costmap_, true);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -238,7 +238,7 @@ mbf_abstract_nav::AbstractControllerExecution::Ptr CostmapNavigationServer::newC
findWithDefault(controller_name_to_costmap_ptr_, plugin_name, local_costmap_ptr_);
return boost::make_shared<mbf_costmap_nav::CostmapControllerExecution>(
plugin_name, boost::static_pointer_cast<mbf_costmap_core::CostmapController>(plugin_ptr), robot_info_, vel_pub_,
goal_pub_, costmap_ptr, last_config_);
costmap_ptr, last_config_);
}

mbf_abstract_nav::AbstractRecoveryExecution::Ptr CostmapNavigationServer::newRecoveryExecution(
Expand Down

0 comments on commit 3c7b7f1

Please sign in to comment.