/** This callback is triggered when someone sends an action command to the
* "move_to" server. */
void moveToCallback(const amr_msgs::MoveToGoalConstPtr& goal)
{
ROS_INFO("Received [move_to] action command.");
if (!setNewGoal(goal))
return;
// Start an infinite loop where in each iteration we try to advance towards
// the goal and also check if the goal has been preempted (i.e. a new goal
// was given). The loop is terminated if the goal was reached, or if the
// node itself shuts down.
ros::Rate rate(controller_frequency_);
ros::NodeHandle nh;
while (nh.ok())
{
// Exit if the goal was aborted
if (!move_to_server_->isActive())
return;
// Process pending preemption requests
if (move_to_server_->isPreemptRequested())
{
ROS_INFO("Action preemption requested.");
if (move_to_server_->isNewGoalAvailable() && setNewGoal(move_to_server_->acceptNewGoal()))
{
// A new valid goal was given and accepted. Notify the ActionServer
// that we preempted and proceed to execute the action.
//move_to_server_->setPreempted();
}
else
{
// We have been preempted explicitly, without a new goal therefore we
// need to shut things down
publishZeroVelocity();
// Notify the ActionServer that we have successfully preempted
move_to_server_->setPreempted();
// No goal - no actions, just exit the callback
return;
}
}
// Issue a command to move towards the current goal
if (!moveTowardsGoal())
{
// Finish execution if the goal was reached
move_to_server_->setSucceeded(amr_msgs::MoveToResult(), "Goal reached.");
publishZeroVelocity();
return;
}
rate.sleep();
}
// We get here only if nh.ok() returned false, i.e. the node has received
// a shutdown request
move_to_server_->setAborted(amr_msgs::MoveToResult(), "Aborted. The node has been killed.");
}
void
BKPlanner::goalCB(const PoseWithCovarianceStamped::ConstPtr& goal_cov_ptr)
{
sc_.stopAll();
// Get the covariance
double cov = goal_cov_ptr->pose.covariance[0];
// Then strip it out so we are left with a regular PoseStamped
PoseStamped goal;
goal.header = goal_cov_ptr->header;
goal.pose = goal_cov_ptr->pose.pose;
ROS_INFO_THROTTLE(2,"[Goal callback] Got new goal: (%.2f,%.2f) in frame %s, covariance %.2f", goal.pose.position.x, goal.pose.position.y, goal.header.frame_id.c_str(), cov);
// Try to transform the goal to my frame of reference
PoseStamped new_goal;
if( !poseToGlobalFrame(goal, new_goal) )
{
ROS_ERROR("[Goal callback] Could not transform goal to global frame");
return;
}
// Accept the new goal under two conditions:
// 1) it is far enough away from the last accepted goal (goal_hysteresis_)
// 2) enough time has elapsed since the last time (goal_timeout_)
double d = dist(latest_goal_, new_goal);
ros::Duration time_since_goal = last_accepted_goal_ - ros::Time::now();
if( d > goal_hysteresis_ || time_since_goal > goal_timeout_ || planner_->override_goal_generation_ )
{
sc_.say("New goal.");
setNewGoal(new_goal);
last_accepted_goal_ = ros::Time::now();
}
}
BKPlanner::BKPlanner(std::string name, tf::TransformListener& tf):
nh_ (),
priv_nh_ ("~"),
tf_ (tf),
got_new_goal_ (false),
last_accepted_goal_ (ros::Time(0)),
sc_ ()
{
// Initialize the cost map and path checker
planner_costmap_ = shared_ptr<costmap_2d::Costmap2DROS>
(new costmap_2d::Costmap2DROS("local_costmap", tf_) );
path_checker_ = shared_ptr<path_checker::PathChecker>
(new path_checker::PathChecker("path_checker", planner_costmap_));
// Get planner-related parameters
priv_nh_.param("goal_hysteresis", goal_hysteresis_, 0.2);
ROS_INFO("[bk_planner] Goal hysteresis is %.2f", goal_hysteresis_);
double temp;
priv_nh_.param("goal_timeout", temp, 3.0);
goal_timeout_ = ros::Duration(temp);
ROS_INFO("[bk_planner] Goal timeout is %.2fsec", temp);
priv_nh_.param("goal_cov_thresh", goal_cov_thresh_, 1.0);
ROS_INFO("[bk_planner] Goal covariance threshold is %.2f", goal_cov_thresh_);
// Get the robot's current pose and set a goal there
PoseStamped robot_pose;
if( getRobotPose(robot_pose) ) {
setNewGoal(robot_pose);
}
else {
ROS_ERROR("[bk_planner] Could not transform start pose");
}
// Create the planner and feeder threads
planner_ = shared_ptr<BKPlanningThread>
(new BKPlanningThread(this) );
feeder_ = shared_ptr<BKFeederThread>
(new BKFeederThread (this) );
// Set their references to each other
planner_->setFeeder (feeder_);
feeder_ ->setPlanner(planner_);
// Kick off the threads
planning_thread_ = shared_ptr<boost::thread>
(new boost::thread(boost::bind(&BKPlanningThread::run, planner_)) );
feeder_thread_ = shared_ptr<boost::thread>
(new boost::thread(boost::bind(&BKFeederThread::run , feeder_ )) );
// This node subscribes to a goal pose.
goal_sub_ = nh_.subscribe("goal_with_covariance", 1, &BKPlanner::goalCB, this);
// ROS_INFO("BKPlanner constructor finished");
return;
}
void Division::changeGoal(const int newGoal)
{
(newGoal == 113) ? setNewGoal(255) : setNewGoal(newGoal);
return;
}
