Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14032] pkg/trunk
|
From: <ei...@us...> - 2009-04-17 21:40:00
|
Revision: 14032
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14032&view=rev
Author: eitanme
Date: 2009-04-17 21:39:55 +0000 (Fri, 17 Apr 2009)
Log Message:
-----------
Provide feedback in move base action, rotate to goal in controller and not move base action
Modified Paths:
--------------
pkg/trunk/highlevel/nav/include/nav/move_base_action.h
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-17 21:39:55 UTC (rev 14032)
@@ -45,7 +45,6 @@
#include <costmap_2d/costmap_2d.h>
#include <navfn/navfn_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>
-#include <angles/angles.h>
#include <vector>
#include <string>
@@ -108,34 +107,6 @@
void prunePlan();
/**
- * @brief Once a goal is reached... rotate to the goal orientation
- * @param cmd_vel The velocity commands to be filled
- */
- void rotateToGoal(robot_msgs::PoseDot& cmd_vel);
-
- /**
- * @brief Check if the goal orientation has been achieved
- * @return True if achieved, false otherwise
- */
- bool goalOrientationReached();
-
- /**
- * @brief Check if the goal position has been achieved
- * @return True if achieved, false otherwise
- */
- bool goalPositionReached();
-
- /**
- * @brief Compute the distance between two points
- * @param x1 The first x point
- * @param y1 The first y point
- * @param x2 The second x point
- * @param y2 The second y point
- * @return
- */
- double distance(double x1, double y1, double x2, double y2);
-
- /**
* @brief Get the current pose of the robot in the global frame and set the global_pose_ variable
*/
void updateGlobalPose();
Modified: pkg/trunk/highlevel/nav/manifest.xml
===================================================================
--- pkg/trunk/highlevel/nav/manifest.xml 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/highlevel/nav/manifest.xml 2009-04-17 21:39:55 UTC (rev 14032)
@@ -17,7 +17,6 @@
<depend package="tf"/>
<depend package="roslib"/>
<depend package="navfn"/>
- <depend package="angles"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnav"/>
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-17 21:39:55 UTC (rev 14032)
@@ -169,6 +169,9 @@
std::vector<robot_actions::Pose2D> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
+ //we'll also push the goal point onto the end of the plan to make sure orientation is taken into account
+ global_plan.push_back(goal);
+
lock_.lock();
//copy over the new global plan
valid_plan_ = valid_plan;
@@ -198,31 +201,6 @@
}
}
- double MoveBaseAction::distance(double x1, double y1, double x2, double y2){
- return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
- }
-
- bool MoveBaseAction::goalPositionReached(){
- double dist = distance(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), goal_.x, goal_.y);
- return fabs(dist) <= xy_goal_tolerance_;
- }
-
- bool MoveBaseAction::goalOrientationReached(){
- double useless_pitch, useless_roll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
- return fabs(angles::shortest_angular_distance(yaw, goal_.th)) <= yaw_goal_tolerance_;
- }
-
- void MoveBaseAction::rotateToGoal(robot_msgs::PoseDot& cmd_vel){
- double uselessPitch, uselessRoll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
- ROS_DEBUG("Moving to desired goal orientation\n");
- cmd_vel.vel.vx = 0;
- cmd_vel.vel.vy = 0;
- double ang_diff = angles::shortest_angular_distance(yaw, goal_.th);
- cmd_vel.ang_vel.vz = ang_diff > 0.0 ? std::max(min_abs_theta_vel_, ang_diff) : std::min(-1.0 * min_abs_theta_vel_, ang_diff);
- }
-
void MoveBaseAction::prunePlan(){
lock_.lock();
std::vector<robot_actions::Pose2D>::iterator it = global_plan_.begin();
@@ -259,6 +237,20 @@
//update the global pose
updateGlobalPose();
+ //update feedback to correspond to our current position
+ double useless_pitch, useless_roll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
+ feedback.header.frame_id = global_frame_;
+ feedback.header.stamp = ros::Time::now();
+ feedback.x = global_pose_.getOrigin().x();
+ feedback.y = global_pose_.getOrigin().y();
+ feedback.z = 0.0;
+ feedback.th = yaw;
+
+ //push the feedback out
+ update(feedback);
+
+
//make sure to update the cost_map we'll use for this cycle
controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
@@ -268,61 +260,51 @@
//prune the plan before we pass it to the controller
prunePlan();
- //check for success
- if(goalPositionReached()){
- if(goalOrientationReached()){
- if(tc_->stopped())
- return robot_actions::SUCCESS;
- }
- else{
- //compute the velocity command we need to send for rotating to the goal
- robot_msgs::PoseDot cmd_vel;
- rotateToGoal(cmd_vel);
- publishFootprint();
- ros_node_.publish("cmd_vel", cmd_vel);
- }
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+
+ //check that the observation buffers for the costmap are current
+ if(!controller_cost_map_ros_->isCurrent()){
+ ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
+ continue;
}
- else {
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
- //check that the observation buffers for the costmap are current
- if(!controller_cost_map_ros_->isCurrent()){
- ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
- continue;
- }
+ bool valid_control = false;
+ robot_msgs::PoseDot cmd_vel;
+ std::vector<robot_actions::Pose2D> local_plan;
+ //pass plan to controller
+ lock_.lock();
+ if(valid_plan_){
+ //get observations for the non-costmap controllers
+ std::vector<Observation> observations;
+ controller_cost_map_ros_->getMarkingObservations(observations);
+ valid_control = tc_->computeVelocityCommands(global_plan_, cmd_vel, local_plan, observations);
+ ros_node_.publish("cmd_vel", cmd_vel);
+ }
+ lock_.unlock();
- bool valid_control = false;
- robot_msgs::PoseDot cmd_vel;
- std::vector<robot_actions::Pose2D> local_plan;
- //pass plan to controller
- lock_.lock();
- if(valid_plan_){
- //get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_cost_map_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(global_plan_, cmd_vel, local_plan, observations);
- ros_node_.publish("cmd_vel", cmd_vel);
- }
- lock_.unlock();
+ //check for success
+ if(tc_->goalReached())
+ return robot_actions::SUCCESS;
- //if we don't have a valid control... we need to re-plan explicitly
- if(!valid_control){
- makePlan(goal_);
- }
- //for visualization purposes
- publishPath(global_plan_, "gui_path", 0.0, 1.0, 0.0, 0.0);
- publishPath(local_plan, "local_path", 0.0, 0.0, 1.0, 0.0);
- publishFootprint();
-
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- ROS_DEBUG("Full control cycle: %.9f Valid control: %d, Vel Cmd (%.2f, %.2f, %.2f)", t_diff, valid_control, cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.vel.vz);
+ //if we don't have a valid control... we need to re-plan explicitly
+ if(!valid_control){
+ makePlan(goal_);
}
+
+ //for visualization purposes
+ publishPath(global_plan_, "gui_path", 0.0, 1.0, 0.0, 0.0);
+ publishPath(local_plan, "local_path", 0.0, 0.0, 1.0, 0.0);
+ publishFootprint();
+
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_DEBUG("Full control cycle: %.9f Valid control: %d, Vel Cmd (%.2f, %.2f, %.2f)", t_diff, valid_control, cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.vel.vz);
+
//sleep the remainder of the cycle
if(!sleepLeftover(start_time, cycle_time))
ROS_WARN("Controll loop missed its desired cycle time of %.4f", cycle_time.toSec());
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-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-17 21:39:55 UTC (rev 14032)
@@ -63,6 +63,8 @@
#include <string>
+#include <angles/angles.h>
+
namespace base_local_planner {
/**
* @class TrajectoryPlannerROS
@@ -118,16 +120,58 @@
void getLocalGoal(double& x, double& y);
/**
+ * @brief Check if the goal pose has been achieved
+ * @return True if achieved, false otherwise
+ */
+ bool goalReached();
+
+ private:
+ /**
* @brief Check whether the robot is stopped or not
* @return True if the robot is stopped, false otherwise
*/
bool stopped();
+ /**
+ * @brief Check if the goal orientation has been achieved
+ * @param global_pose The pose of the robot in the global frame
+ * @param goal_x The desired x value for the goal
+ * @param goal_y The desired y value for the goal
+ * @return True if achieved, false otherwise
+ */
+ bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th);
+
+ /**
+ * @brief Check if the goal position has been achieved
+ * @param global_pose The pose of the robot in the global frame
+ * @param goal_th The desired th value for the goal
+ * @return True if achieved, false otherwise
+ */
+ bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y);
+
+ /**
+ * @brief Once a goal position is reached... rotate to the goal orientation
+ * @param global_pose The pose of the robot in the global frame
+ * @param goal_th The desired th value for the goal
+ * @param cmd_vel The velocity commands to be filled
+ */
+ void rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, double goal_th, robot_msgs::PoseDot& cmd_vel);
+
+ /**
+ * @brief Compute the distance between two points
+ * @param x1 The first x point
+ * @param y1 The first y point
+ * @param x2 The second x point
+ * @param y2 The second y point
+ * @return
+ */
+ double distance(double x1, double y1, double x2, double y2);
+
void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void tiltScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void odomCallback();
- private:
+
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
@@ -143,6 +187,8 @@
deprecated_msgs::RobotBase2DOdom odom_msg_, base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
+ double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
+ bool goal_reached_;
};
};
Modified: pkg/trunk/nav/base_local_planner/manifest.xml
===================================================================
--- pkg/trunk/nav/base_local_planner/manifest.xml 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/nav/base_local_planner/manifest.xml 2009-04-17 21:39:55 UTC (rev 14032)
@@ -16,6 +16,7 @@
<depend package="costmap_2d" />
<depend package="robot_actions" />
<depend package="voxel_grid" />
+ <depend package="angles" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-17 21:39:55 UTC (rev 14032)
@@ -48,18 +48,21 @@
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
const Costmap2D& cost_map, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
: world_model_(NULL), tc_(NULL), base_scan_notifier_(NULL), tf_(tf), laser_scans_(2),
- point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2){
+ point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true){
double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
int vx_samples, vtheta_samples;
double pdist_scale, gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist;
bool holonomic_robot, dwa, simple_attractor, heading_scoring;
double heading_scoring_timestep;
- double max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th;
+ double max_vel_x, min_vel_x, max_vel_th, min_vel_th;
string world_model_type;
ros_node.param("~base_local_planner/global_frame", global_frame_, string("map"));
ros_node.param("~base_local_planner/robot_base_frame", robot_base_frame_, string("base_link"));
+ ros_node.param("~base_local_planner/yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
+ ros_node.param("~base_local_planner/xy_goal_tolerance", xy_goal_tolerance_, 0.10);
+
string odom_topic;
ros_node.param("~base_local_planner/odom_topic", odom_topic, string("odom"));
// Subscribe to odometry messages to get global pose
@@ -95,7 +98,7 @@
ros_node.param("~base_local_planner/min_vel_x", min_vel_x, 0.1);
ros_node.param("~base_local_planner/max_vel_th", max_vel_th, 1.0);
ros_node.param("~base_local_planner/min_vel_th", min_vel_th, -1.0);
- ros_node.param("~base_local_planner/min_in_place_vel_th", min_in_place_vel_th, 0.4);
+ ros_node.param("~base_local_planner/min_in_place_vel_th", min_in_place_vel_th_, 0.4);
ros_node.param("~base_local_planner/world_model", world_model_type, string("freespace"));
ros_node.param("~base_local_planner/dwa", dwa, false);
ros_node.param("~base_local_planner/heading_scoring", heading_scoring, false);
@@ -148,7 +151,7 @@
tc_ = new TrajectoryPlanner(*world_model_, cost_map, footprint_spec, inscribed_radius, circumscribed_radius,
acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, holonomic_robot,
- max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th,
+ max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_,
dwa, heading_scoring, heading_scoring_timestep, simple_attractor);
}
@@ -240,6 +243,31 @@
&& abs(base_odom_.vel.y) <= trans_stopped_velocity_;
}
+ double TrajectoryPlannerROS::distance(double x1, double y1, double x2, double y2){
+ return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
+ }
+
+ bool TrajectoryPlannerROS::goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y){
+ double dist = distance(global_pose.getOrigin().x(), global_pose.getOrigin().y(), goal_x, goal_y);
+ return fabs(dist) <= xy_goal_tolerance_;
+ }
+
+ bool TrajectoryPlannerROS::goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th){
+ double useless_pitch, useless_roll, yaw;
+ global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
+ return fabs(angles::shortest_angular_distance(yaw, goal_th)) <= yaw_goal_tolerance_;
+ }
+
+ void TrajectoryPlannerROS::rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, double goal_th, robot_msgs::PoseDot& cmd_vel){
+ double uselessPitch, uselessRoll, yaw;
+ global_pose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ ROS_DEBUG("Moving to desired goal orientation\n");
+ cmd_vel.vel.vx = 0;
+ cmd_vel.vel.vy = 0;
+ double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
+ cmd_vel.ang_vel.vz = ang_diff > 0.0 ? std::max(min_in_place_vel_th_, ang_diff) : std::min(-1.0 * min_in_place_vel_th_, ang_diff);
+ }
+
void TrajectoryPlannerROS::odomCallback(){
base_odom_.lock();
@@ -267,6 +295,10 @@
base_odom_.unlock();
}
+ bool TrajectoryPlannerROS::goalReached(){
+ return goal_reached_;
+ }
+
bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_actions::Pose2D>& global_plan,
robot_msgs::PoseDot& cmd_vel,
std::vector<robot_actions::Pose2D>& local_plan,
@@ -315,6 +347,36 @@
gettimeofday(&start, NULL);
*/
+ //we assume the global goal is the last point in the global plan
+ double goal_x = global_plan.back().x;
+ double goal_y = global_plan.back().y;
+ double goal_th = global_plan.back().th;
+
+ //assume at the beginning of our control cycle that we could have a new goal
+ goal_reached_ = false;
+
+ //check to see if we've reached the goal position
+ if(goalPositionReached(global_pose, goal_x, goal_y)){
+ //check to see if the goal orientation has been reached
+ if(goalOrientationReached(global_pose, goal_th)){
+ //set the velocity command to zero
+ cmd_vel.vel.vx = 0.0;
+ cmd_vel.vel.vy = 0.0;
+ cmd_vel.ang_vel.vz = 0.0;
+
+ //make sure that we're actually stopped before returning success
+ if(stopped())
+ goal_reached_ = true;
+ }
+ else {
+ //otherwise we need to rotate to the goal... compute the next velocity we should take
+ rotateToGoal(global_pose, goal_th, cmd_vel);
+ }
+
+ //we don't actually want to run the controller when we're just rotating to goal
+ return true;
+ }
+
tc_->updatePlan(global_plan);
obs_lock_.lock();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
