Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22876] pkg/trunk
|
From: <ei...@us...> - 2009-08-25 19:06:21
|
Revision: 22876
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22876&view=rev
Author: eitanme
Date: 2009-08-25 19:06:06 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Making some changes as per the base_local_planner api review
Modified Paths:
--------------
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -246,7 +246,7 @@
ROS_INFO ("Passing plan to controller");
if(new_plan_){
new_plan_ = false;
- if(!tc_->updatePlan(global_plan_)){
+ if(!tc_->setPlan(global_plan_)){
resetState();
ROS_WARN("move_base aborted because it failed to pass the plan from the planner to the controller");
return robot_actions::ABORTED;
@@ -260,7 +260,7 @@
last_valid_control_ = ros::Time::now();
//check for success
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
if(attempted_rotation_){
valid_control = false;
if(done_half_rotation_){
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -273,7 +273,7 @@
rotate_plan.push_back(rotate_goal_msg);
//pass the rotation goal to the controller
- if(!tc_->updatePlan(rotate_plan)){
+ if(!tc_->setPlan(rotate_plan)){
ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
return false;
}
@@ -432,7 +432,7 @@
case PLANNING:
ROS_DEBUG("In planning state");
if(makePlan(goal, global_plan)){
- if(!tc_->updatePlan(global_plan)){
+ if(!tc_->setPlan(global_plan)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
@@ -466,7 +466,7 @@
ROS_DEBUG("In controlling state");
//check to see if we've reached our goal
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
ROS_DEBUG("Goal reached!");
resetState();
as_.setSucceeded();
@@ -520,7 +520,7 @@
break;
case EXECUTE_ROTATE_1:
ROS_DEBUG("In in-place rotation state 1");
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
clearing_state_ = IN_PLACE_ROTATION_2;
}
else if(!rotateRobot()){
@@ -540,7 +540,7 @@
break;
case EXECUTE_ROTATE_2:
ROS_DEBUG("In in-place rotation state 2");
- if(tc_->goalReached() || !rotateRobot()){
+ if(tc_->isGoalReached() || !rotateRobot()){
clearing_state_ = AGGRESSIVE_RESET;
state_ = PLANNING;
}
Modified: pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-25 19:06:06 UTC (rev 22876)
@@ -103,15 +103,6 @@
~TrajectoryPlannerROS();
/**
- * @brief Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
- * @param x_i The x position of the robot
- * @param y_i The y position of the robot
- * @param theta_i The orientation of the robot
- * @return A vector of points in world coordinates that correspond to the verticies of the robot's footprint
- */
- std::vector<geometry_msgs::Point> drawFootprint(double x_i, double y_i, double theta_i);
-
- /**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
@@ -119,28 +110,29 @@
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
- * @brief Update the plan that the controller is following
+ * @brief Set 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
*/
- bool updatePlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
+ bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
- * @brief Returns the local goal the robot is pursuing
- * @param x Will be set to the x position of the goal in world coordinates
- * @param y Will be set to the y position of the goal in world coordinates
- * @return
- */
- void getLocalGoal(double& x, double& y);
-
- /**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
- bool goalReached();
+ bool isGoalReached();
private:
/**
+ * @brief Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
+ * @param x_i The x position of the robot
+ * @param y_i The y position of the robot
+ * @param theta_i The orientation of the robot
+ * @return A vector of points in world coordinates that correspond to the verticies of the robot's footprint
+ */
+ std::vector<geometry_msgs::Point> drawFootprint(double x_i, double y_i, double theta_i);
+
+ /**
* @brief Check whether the robot is stopped or not
* @return True if the robot is stopped, false otherwise
*/
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -236,7 +236,7 @@
}
}
- bool TrajectoryPlannerROS::goalReached(){
+ bool TrajectoryPlannerROS::isGoalReached(){
if(!initialized_){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
@@ -305,7 +305,7 @@
return false;
}
- bool TrajectoryPlannerROS::updatePlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
+ bool TrajectoryPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
if(!initialized_){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
@@ -514,16 +514,6 @@
return true;
}
- void TrajectoryPlannerROS::getLocalGoal(double& x, double& y){
- if(!initialized_){
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
- return;
- }
-
- tc_->getLocalGoal(x, y);
- return;
- }
-
void TrajectoryPlannerROS::prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
Modified: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-25 19:06:06 UTC (rev 22876)
@@ -49,8 +49,8 @@
#include <vector>
#include <string>
#include <nav_msgs/GetPlan.h>
+#include <geometry_msgs/Twist.h>
#include <visualization_msgs/Marker.h>
-#include <geometry_msgs/Twist.h>
#include <pluginlib/class_loader.h>
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -327,7 +327,7 @@
rotate_plan.push_back(rotate_goal_msg);
//pass the rotation goal to the controller
- if(!tc_->updatePlan(rotate_plan)){
+ if(!tc_->setPlan(rotate_plan)){
ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
return false;
}
@@ -482,7 +482,7 @@
case PLANNING:
ROS_DEBUG("In planning state");
if(makePlan(goal, global_plan)){
- if(!tc_->updatePlan(global_plan)){
+ if(!tc_->setPlan(global_plan)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
@@ -516,7 +516,7 @@
ROS_DEBUG("In controlling state");
//check to see if we've reached our goal
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
ROS_DEBUG("Goal reached!");
resetState();
as_.setSucceeded();
@@ -570,7 +570,7 @@
break;
case EXECUTE_ROTATE_1:
ROS_DEBUG("In in-place rotation state 1");
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
clearing_state_ = IN_PLACE_ROTATION_2;
}
else if(!rotateRobot()){
@@ -590,7 +590,7 @@
break;
case EXECUTE_ROTATE_2:
ROS_DEBUG("In in-place rotation state 2");
- if(tc_->goalReached() || !rotateRobot()){
+ if(tc_->isGoalReached() || !rotateRobot()){
clearing_state_ = AGGRESSIVE_RESET;
state_ = PLANNING;
}
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -318,13 +318,13 @@
rotate_plan.push_back(rotate_goal_msg);
//pass the rotation goal to the controller
- if(!tc_->updatePlan(rotate_plan)){
+ if(!tc_->setPlan(rotate_plan)){
ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
return;
}
geometry_msgs::Twist cmd_vel;
- while(!isPreemptRequested() && ros_node_.ok() && !tc_->goalReached()){
+ while(!isPreemptRequested() && ros_node_.ok() && !tc_->isGoalReached()){
if(tc_->computeVelocityCommands(cmd_vel)){
//make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
@@ -403,7 +403,7 @@
case PLANNING:
ROS_DEBUG("In planning state");
if(makePlan(goal, global_plan)){
- if(!tc_->updatePlan(global_plan)){
+ if(!tc_->setPlan(global_plan)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
@@ -434,7 +434,7 @@
ROS_DEBUG("In controlling state");
//check to see if we've reached our goal
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
ROS_DEBUG("Goal reached!");
resetState();
return robot_actions::SUCCESS;
Modified: pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h
===================================================================
--- pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h 2009-08-25 19:06:06 UTC (rev 22876)
@@ -45,8 +45,8 @@
class BaseLocalPlanner{
public:
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
- virtual bool goalReached() = 0;
- virtual bool updatePlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
+ virtual bool isGoalReached() = 0;
+ virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
