Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[15353] pkg/trunk
|
From: <stu...@us...> - 2009-05-13 22:33:54
|
Revision: 15353
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=15353&view=rev
Author: stuglaser
Date: 2009-05-13 22:33:50 +0000 (Wed, 13 May 2009)
Log Message:
-----------
Nav stack can now take a goal in any frame
Modified Paths:
--------------
pkg/trunk/highlevel/nav/src/move_base.cpp
pkg/trunk/highlevel/nav/src/move_base_local.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
Modified: pkg/trunk/highlevel/nav/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base.cpp 2009-05-13 21:55:44 UTC (rev 15352)
+++ pkg/trunk/highlevel/nav/src/move_base.cpp 2009-05-13 22:33:50 UTC (rev 15353)
@@ -250,13 +250,15 @@
bool valid_control = false;
//pass plan to controller
- lock_.lock();
+
if(valid_plan_){
//if we have a new plan... we'll update the plan for the controller
if(new_plan_){
- tc_->updatePlan(global_plan_);
new_plan_ = false;
+ if(!tc_->updatePlan(global_plan_))
+ return robot_actions::ABORTED;
}
+
//get observations for the non-costmap controllers
std::vector<Observation> observations;
controller_costmap_ros_->getMarkingObservations(observations);
@@ -297,8 +299,6 @@
//give the base the velocity command
ros_node_.publish("cmd_vel", cmd_vel);
- lock_.unlock();
-
//if we don't have a valid control... we need to re-plan explicitly
if(!valid_control){
ros::Duration patience = ros::Duration(controller_patience_);
Modified: pkg/trunk/highlevel/nav/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-05-13 21:55:44 UTC (rev 15352)
+++ pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-05-13 22:33:50 UTC (rev 15353)
@@ -190,7 +190,8 @@
//pass plan to controller
std::vector<robot_msgs::PoseStamped> global_plan;
global_plan.push_back(goal);
- tc_->updatePlan(global_plan);
+ if(!tc_->updatePlan(global_plan))
+ return robot_actions::ABORTED;
//get observations for the non-costmap controllers
std::vector<Observation> observations;
controller_costmap_ros_->getMarkingObservations(observations);
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-05-13 21:55:44 UTC (rev 15352)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-05-13 22:33:50 UTC (rev 15353)
@@ -112,8 +112,9 @@
/**
* @brief Update the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
+ * @return True if the plan was updated successfully, false otherwise
*/
- void updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan);
+ bool updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Returns the local goal the robot is pursuing
@@ -229,7 +230,7 @@
bool goal_reached_;
costmap_2d::Costmap2DPublisher* costmap_publisher_;
std::vector<robot_msgs::PoseStamped> global_plan_;
- double transform_tolerance_;
+ double transform_tolerance_, update_plan_tolerance_;
};
};
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-05-13 21:55:44 UTC (rev 15352)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-05-13 22:33:50 UTC (rev 15353)
@@ -67,6 +67,7 @@
ros_node.param("~base_local_planner/costmap/global_frame", global_frame_, string("map"));
ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
ros_node.param("~base_local_planner/transform_tolerance", transform_tolerance_, 0.2);
+ ros_node.param("~base_local_planner/update_plan_tolerance", update_plan_tolerance_, 1.0);
double map_publish_frequency;
ros_node.param("~base_local_planner/costmap_visualization_rate", map_publish_frequency, 2.0);
@@ -339,7 +340,7 @@
return goal_reached_;
}
- void TrajectoryPlannerROS::updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan){
+ bool TrajectoryPlannerROS::updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan){
//reset the global plan
global_plan_.clear();
@@ -349,31 +350,35 @@
robot_msgs::PoseStamped new_pose;
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
new_pose = orig_global_plan[i];
- new_pose.header.stamp = ros::Time();
+ if(!tf_.canTransform(global_frame_, new_pose.header.frame_id, new_pose.header.stamp, ros::Duration(update_plan_tolerance_))){
+ ROS_ERROR("TrajectoryPlannerROS cannot service your goal because the transform is not available, we waited %.4f seconds", update_plan_tolerance_);
+ return false;
+ }
tf_.transformPose(global_frame_, new_pose, new_pose);
// check global_pose timeout
if (current_time.toSec() - new_pose.header.stamp.toSec() > transform_tolerance_) {
ROS_ERROR("TrajectoryPlannerROS transform timeout updating plan. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
current_time.toSec() ,new_pose.header.stamp.toSec() ,transform_tolerance_);
- return;
+ return false;
}
global_plan_.push_back(new_pose);
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return;
+ return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return;
+ return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (orig_global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame_.c_str(),orig_global_plan.size(), orig_global_plan[0].header.frame_id.c_str());
- return;
+ return false;
}
+ return true;
}
bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
