Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20621] pkg/trunk
|
From: <ei...@us...> - 2009-08-04 03:19:37
|
Revision: 20621
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20621&view=rev
Author: eitanme
Date: 2009-08-04 03:19:26 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Changes to costmap_2d as per the api review. It is now api cleared.
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml
pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml
pkg/trunk/demos/door_demos/launch/move_base_door.launch
pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
pkg/trunk/motion_planning/mpglue/src/costmap.cpp
pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch
pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml
pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.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.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation.h
pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation_buffer.h
pkg/trunk/stacks/navigation/costmap_2d/launch_costmap_2d_ros.xml
pkg/trunk/stacks/navigation/costmap_2d/mainpage.dox
pkg/trunk/stacks/navigation/costmap_2d/manifest.xml
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/observation_buffer.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/voxel_costmap_2d.cpp
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/navfn/src/navfn_node.cpp
pkg/trunk/stacks/navigation/navfn/src/navfn_ros.cpp
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -16,7 +16,7 @@
lethal_cost_threshold: 100
# BEGIN VOXEL STUFF
-observation_topics: base_scan_marking base_scan tilt_scan ground_object_cloud
+observation_sources: base_scan_marking base_scan tilt_scan ground_object_cloud
base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0}
Modified: pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -15,6 +15,6 @@
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
-observation_topics: scan
-scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
+observation_sources: scan
+scan: {data_type: LaserScan, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
Modified: pkg/trunk/demos/door_demos/launch/move_base_door.launch
===================================================================
--- pkg/trunk/demos/door_demos/launch/move_base_door.launch 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/demos/door_demos/launch/move_base_door.launch 2009-08-04 03:19:26 UTC (rev 20621)
@@ -31,24 +31,21 @@
<param name="costmap/max_obstacle_height" value="2.0" />
<param name="costmap/raytrace_range" value="1.5" />
- <!--param name="costmap/observation_topics" value="base_scan base_scan_marking" /-->
- <param name="costmap/observation_topics" value="base_scan base_scan_marking" />
+ <!--param name="costmap/observation_sources" value="base_scan base_scan_marking" /-->
+ <param name="costmap/observation_sources" value="base_scan base_scan_marking" />
- <param name="costmap/base_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan/observation_persistence" value="0.0" />
<param name="costmap/base_scan/expected_update_rate" value="0.2" />
<param name="costmap/base_scan/data_type" value="LaserScan" />
<param name="costmap/base_scan/clearing" value="true" />
<param name="costmap/base_scan/marking" value="false" />
- <param name="costmap/base_scan_marking/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan_marking/observation_persistence" value="0.0" />
<param name="costmap/base_scan_marking/expected_update_rate" value="0.2" />
<param name="costmap/base_scan_marking/data_type" value="PointCloud" />
<param name="costmap/base_scan_marking/clearing" value="false" />
<param name="costmap/base_scan_marking/marking" value="true" />
- <param name="costmap/tilt_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/tilt_scan/observation_persistence" value="0.0" />
<param name="costmap/tilt_scan/expected_update_rate" value="0.2" />
<param name="costmap/tilt_scan/data_type" value="LaserScan" />
Modified: pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/demos/erratic_gazebo/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -9,7 +9,7 @@
cost_scaling_factor: 25.0
lethal_cost_threshold: 100
- observation_topics: base_scan_marking base_scan
+ observation_sources: base_scan_marking base_scan
base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstalce_height: 2.0}
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -98,7 +98,7 @@
//initialize the door opening planner
planner_ = new DoorReactivePlanner(ros_node_, tf_,&planner_cost_map_,control_frame_,global_frame_);
- ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.cellSizeX(), planner_cost_map_.cellSizeY());
+ ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.getSizeInCellsX(), planner_cost_map_.getSizeInCellsY());
ros_node_.advertise<manipulation_msgs::JointTraj>(control_topic_name_, 1);
last_diagnostics_publish_time_ = ros::Time::now();
Modified: pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -15,6 +15,6 @@
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
-observation_topics: base_scan
-base_scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
+observation_sources: base_scan
+base_scan: {data_type: LaserScan, expected_update_rate: 0.2,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
Modified: pkg/trunk/motion_planning/mpglue/src/costmap.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -59,14 +59,14 @@
{ return (*get_costmap_)()->getCircumscribedCost(); }
virtual ssize_t getXBegin() const { return 0; }
- virtual ssize_t getXEnd() const { return (*get_costmap_)()->cellSizeX(); }
+ virtual ssize_t getXEnd() const { return (*get_costmap_)()->getSizeInCellsX(); }
virtual ssize_t getYBegin() const { return 0; }
- virtual ssize_t getYEnd() const { return (*get_costmap_)()->cellSizeY(); }
+ virtual ssize_t getYEnd() const { return (*get_costmap_)()->getSizeInCellsY(); }
virtual bool isValidIndex(ssize_t index_x, ssize_t index_y) const {
costmap_2d::Costmap2D const * cm((*get_costmap_)());
- return (index_x >= 0) && (static_cast<size_t>(index_x) < cm->cellSizeX())
- && (index_y >= 0) && (static_cast<size_t>(index_y) < cm->cellSizeY()); }
+ return (index_x >= 0) && (static_cast<size_t>(index_x) < cm->getSizeInCellsX())
+ && (index_y >= 0) && (static_cast<size_t>(index_y) < cm->getSizeInCellsY()); }
virtual bool isLethal(ssize_t index_x, ssize_t index_y,
bool out_of_bounds_reply) const {
@@ -143,8 +143,8 @@
virtual void globalToLocal(double global_x, double global_y, double global_th,
double * local_x, double * local_y, double * local_th) const {
- *local_x = global_x - (*get_costmap_)()->originX();
- *local_y = global_y - (*get_costmap_)()->originY();
+ *local_x = global_x - (*get_costmap_)()->getOriginX();
+ *local_y = global_y - (*get_costmap_)()->getOriginY();
*local_th = global_th;
}
@@ -166,18 +166,18 @@
virtual void localToGlobal(double local_x, double local_y, double local_th,
double * global_x, double * global_y, double * global_th) const {
- *global_x = local_x + (*get_costmap_)()->originX();
- *global_y = local_y + (*get_costmap_)()->originY();
+ *global_x = local_x + (*get_costmap_)()->getOriginX();
+ *global_y = local_y + (*get_costmap_)()->getOriginY();
*global_th = local_th;
}
virtual void getOrigin(double * ox, double * oy, double * oth) const {
- *ox = (*get_costmap_)()->originX();
- *oy = (*get_costmap_)()->originY();
+ *ox = (*get_costmap_)()->getOriginX();
+ *oy = (*get_costmap_)()->getOriginY();
*oth = 0;
}
- virtual double getResolution() const { return (*get_costmap_)()->resolution(); }
+ virtual double getResolution() const { return (*get_costmap_)()->getResolution(); }
mpglue::costmap_2d_getter * get_costmap_;
};
Modified: pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -174,8 +174,8 @@
unsigned int bbx1(std::numeric_limits<unsigned int>::min());
unsigned int bby0(std::numeric_limits<unsigned int>::max());
unsigned int bby1(std::numeric_limits<unsigned int>::min());
- unsigned int const sizex(cm_->cellSizeX());
- unsigned int const sizey(cm_->cellSizeY());
+ unsigned int const sizex(cm_->getSizeInCellsX());
+ unsigned int const sizey(cm_->getSizeInCellsY());
if (removed_obstacle_indices) {
for (index_collection_t::const_iterator irem(removed_obstacle_indices->begin());
@@ -255,8 +255,8 @@
// wastes some operations, but if you want efficiency you
// probably want to use costmap_2d::Costmap2D directly anyway,
// without passing through mpglue.
- double const resolution(cm_->resolution());
- double const inflation2(2 * cm_->inflationRadius());
+ double const resolution(cm_->getResolution());
+ double const inflation2(2 * cm_->getInflationRadius());
double const wx(resolution * 0.5 * (bbx0 + bbx1));
double const wy(resolution * 0.5 * (bby0 + bby1));
double const w_size_x(resolution * (bbx1 - bbx0) + inflation2);
Modified: pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/sbpl_door_action_planner.launch 2009-08-04 03:19:26 UTC (rev 20621)
@@ -15,9 +15,8 @@
<param name="costmap/max_obstacle_height" value="2.0" />
<param name="costmap/raytrace_range" value="3.0" />
- <param name="costmap/observation_topics" value="base_scan" />
+ <param name="costmap/observation_sources" value="base_scan" />
- <param name="costmap/base_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan/observation_persistence" value="0.0" />
<param name="costmap/base_scan/expected_update_rate" value="0.2" />
<param name="costmap/base_scan/data_type" value="LaserScan" />
Modified: pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml
===================================================================
--- pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/nav/nav_apps/costmap_2d_tutorials/launch_example/costmap_params.yaml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -42,20 +42,20 @@
circumscribed_radius: 0.46
inflation_radius: 0.55
-#The observation_topics list allows users of a Costmap2DROS object to specify what topics should be used
+#The observation_sources list allows users of a Costmap2DROS object to specify what topics should be used
#in conjunction with the underlying Costmap2D object. Topics should be listed here, separated by spaces and
#should be configured in their individual namespaces as shown below. Each topic in this list will be subscribed
#to and managed by the Costmap2DROS object subject to its configuration settings.
-observation_topics: base_scan
+observation_sources: base_scan
-#This is an example of topic configuration for the base_scan topic listed in the observation_topics list above.
+#This is an example of topic configuration for the base_scan topic listed in the observation_sources list above.
#Notice that the base scan receives its own namespace to read its configuration from.
base_scan:
#The sensor_frame parameter specifies what frame the origin of the sensor is
-#assumed to be in. The special value "frame_from_message" infers the origin
+#assumed to be in. Leaving this blank infers the origin
#frame from the frame_id sent in each message over the base_scan topic. Any
#other string will override the frame_id sent in the message.
- sensor_frame: frame_from_message
+ sensor_frame: base_laser
#The data_type parameter specifies what message type to expect for the topic. The
#two supported message types at this point are LaserScan and PointCloud.
Modified: pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp
===================================================================
--- pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/nav/nav_apps/costmap_2d_tutorials/src/costmap_test.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -44,7 +44,7 @@
costmap_ros.getCostmapCopy(costmap);
//do whatever you want with the map here
- ROS_INFO("The size of the map in meters is: (%.2f, %.2f)", costmap.metersSizeX(), costmap.metersSizeY());
+ ROS_INFO("The size of the map in meters is: (%.2f, %.2f)", costmap.getSizeInMetersX(), costmap.getSizeInMetersY());
}
int main(int argc, char** argv){
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-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -74,7 +74,7 @@
//initialize the NavFn planner
planner_ = new NavfnROS("NavfnROS", *planner_costmap_ros_);
- ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.cellSizeX(), planner_costmap_.cellSizeY());
+ ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.getSizeInCellsX(), planner_costmap_.getSizeInCellsY());
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS("local_costmap", tf_);
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -50,12 +50,12 @@
costmap_ros_ = new costmap_2d::Costmap2DROS("", tf_);
//we'll get the parameters for the robot radius from the costmap we're associated with
- inscribed_radius_ = costmap_ros_->inscribedRadius();
- circumscribed_radius_ = costmap_ros_->circumscribedRadius();
- inflation_radius_ = costmap_ros_->inflationRadius();
- global_frame_ = costmap_ros_->globalFrame();
- robot_base_frame_ = costmap_ros_->baseFrame();
- footprint_spec_ = costmap_ros_->robotFootprint();
+ inscribed_radius_ = costmap_ros_->getInscribedRadius();
+ circumscribed_radius_ = costmap_ros_->getCircumscribedRadius();
+ inflation_radius_ = costmap_ros_->getInflationRadius();
+ global_frame_ = costmap_ros_->getGlobalFrameID();
+ robot_base_frame_ = costmap_ros_->getBaseFrameID();
+ footprint_spec_ = costmap_ros_->getRobotFootprint();
ros_node_.param("~controller_frequency", controller_frequency_, 10.0);
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-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -212,6 +212,7 @@
bool prune_plan_;
ros::Publisher footprint_pub_, g_plan_pub_, l_plan_pub_;
ros::Subscriber odom_sub_;
+ boost::recursive_mutex odom_lock_;
};
};
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -58,7 +58,7 @@
double backup_vel,
bool dwa, bool heading_scoring, double heading_scoring_timestep, bool simple_attractor,
vector<double> y_vels)
- : map_(costmap.cellSizeX(), costmap.cellSizeY()), costmap_(costmap),
+ : map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()), costmap_(costmap),
world_model_(world_model), footprint_spec_(footprint_spec),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius),
goal_x_(0), goal_y_(0),
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-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -70,8 +70,8 @@
g_plan_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("~global_plan", 1);
l_plan_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("~local_plan", 1);
- global_frame_ = costmap_ros_.globalFrame();
- robot_base_frame_ = costmap_ros_.baseFrame();
+ global_frame_ = costmap_ros_.getGlobalFrameID();
+ robot_base_frame_ = costmap_ros_.getBaseFrameID();
ros_node_.param("~prune_plan", prune_plan_, true);
ros_node_.param("~yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
@@ -86,9 +86,9 @@
odom_sub_ = global_node.subscribe<deprecated_msgs::RobotBase2DOdom>(odom_topic, 1, boost::bind(&TrajectoryPlannerROS::odomCallback, this, _1));
//we'll get the parameters for the robot radius from the costmap we're associated with
- inscribed_radius_ = costmap_ros_.inscribedRadius();
- circumscribed_radius_ = costmap_ros_.circumscribedRadius();
- inflation_radius_ = costmap_ros_.inflationRadius();
+ inscribed_radius_ = costmap_ros_.getInscribedRadius();
+ circumscribed_radius_ = costmap_ros_.getCircumscribedRadius();
+ inflation_radius_ = costmap_ros_.getInflationRadius();
ros_node_.param("~acc_lim_x", acc_lim_x, 2.5);
ros_node_.param("~acc_lim_y", acc_lim_y, 2.5);
@@ -127,7 +127,7 @@
ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
world_model_ = new CostmapModel(costmap_);
- tc_ = new TrajectoryPlanner(*world_model_, costmap_, costmap_ros_.robotFootprint(), inscribed_radius_, circumscribed_radius_,
+ tc_ = new TrajectoryPlanner(*world_model_, costmap_, costmap_ros_.getRobotFootprint(), 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, escape_reset_dist, escape_reset_theta, holonomic_robot,
max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_, backup_vel,
@@ -147,6 +147,7 @@
}
bool TrajectoryPlannerROS::stopped(){
+ boost::recursive_mutex::scoped_lock(odom_lock_);
return abs(base_odom_.vel.th) <= rot_stopped_velocity_
&& abs(base_odom_.vel.x) <= trans_stopped_velocity_
&& abs(base_odom_.vel.y) <= trans_stopped_velocity_;
@@ -193,8 +194,7 @@
}
void TrajectoryPlannerROS::odomCallback(const deprecated_msgs::RobotBase2DOdom::ConstPtr& msg){
- base_odom_.lock();
-
+ boost::recursive_mutex::scoped_lock(odom_lock_);
try
{
tf::Stamped<btVector3> v_in(btVector3(msg->vel.x, msg->vel.y, 0), ros::Time(), msg->header.frame_id), v_out;
@@ -215,8 +215,6 @@
{
ROS_DEBUG("Extrapolation exception");
}
-
- base_odom_.unlock();
}
bool TrajectoryPlannerROS::goalReached(){
@@ -313,7 +311,7 @@
robot_msgs::PoseStamped newer_pose;
//we'll look ahead on the path the size of our window
- unsigned int needed_path_length = std::max(costmap_.cellSizeX(), costmap_.cellSizeY());
+ unsigned int needed_path_length = std::max(costmap_.getSizeInCellsX(), costmap_.getSizeInCellsY());
for(unsigned int i = 0; i < std::min((unsigned int)global_plan_.size(), needed_path_length); ++i){
const robot_msgs::PoseStamped& new_pose = global_plan_[i];
@@ -371,9 +369,12 @@
// Set current velocities from odometry
robot_msgs::PoseDot global_vel;
+
+ odom_lock_.lock();
global_vel.vel.vx = base_odom_.vel.x;
global_vel.vel.vy = base_odom_.vel.y;
global_vel.ang_vel.vz = base_odom_.vel.th;
+ odom_lock_.unlock();
tf::Stamped<tf::Pose> drive_cmds;
drive_cmds.frame_id_ = robot_base_frame_;
Modified: pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp
===================================================================
--- pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/carrot_planner/src/carrot_planner.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -42,14 +42,14 @@
CarrotPlanner::CarrotPlanner(std::string name, costmap_2d::Costmap2DROS& costmap_ros)
: costmap_ros_(costmap_ros){
ros::NodeHandle n(name);
- n.param("~step_size", step_size_, costmap_ros_.resolution());
+ n.param("~step_size", step_size_, costmap_ros_.getResolution());
n.param("~min_dist_from_robot", min_dist_from_robot_, 0.10);
costmap_ros_.getCostmapCopy(costmap_);
world_model_ = new base_local_planner::CostmapModel(costmap_);
//we'll get the parameters for the robot radius from the costmap we're associated with
- inscribed_radius_ = costmap_ros_.inscribedRadius();
- circumscribed_radius_ = costmap_ros_.circumscribedRadius();
- footprint_spec_ = costmap_ros_.robotFootprint();
+ inscribed_radius_ = costmap_ros_.getInscribedRadius();
+ circumscribed_radius_ = costmap_ros_.getCircumscribedRadius();
+ footprint_spec_ = costmap_ros_.getRobotFootprint();
}
@@ -91,9 +91,9 @@
plan.clear();
costmap_ros_.getCostmapCopy(costmap_);
- if(goal.header.frame_id != costmap_ros_.globalFrame()){
+ if(goal.header.frame_id != costmap_ros_.getGlobalFrameID()){
ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.",
- costmap_ros_.globalFrame().c_str(), goal.header.frame_id.c_str());
+ costmap_ros_.getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
return false;
}
Modified: pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -208,13 +208,6 @@
}
/**
- * @brief Will return a copy of the underlying unsigned char array used as the costmap
- * <b>(NOTE: THE BURDEN IS ON THE USER TO DELETE THE ARRAY RETURNED)</b>
- * @return A copy of the underlying unsigned char array storing cost values
- */
- unsigned char* getCharMapCopy() const;
-
- /**
* @brief Will return a immutable pointer to the underlying unsigned char array used as the costmap
* @return A pointer to the underlying unsigned char array storing cost values
*/
@@ -224,73 +217,63 @@
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
- unsigned int cellSizeX() const;
+ unsigned int getSizeInCellsX() const;
/**
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap
*/
- unsigned int cellSizeY() const;
+ unsigned int getSizeInCellsY() const;
/**
* @brief Accessor for the x size of the costmap in meters
* @return The x size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
- double metersSizeX() const;
+ double getSizeInMetersX() const;
/**
* @brief Accessor for the y size of the costmap in meters
* @return The y size of the costmap (returns the centerpoint of the last legal cell in the map)
*/
- double metersSizeY() const;
+ double getSizeInMetersY() const;
/**
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
*/
- double originX() const;
+ double getOriginX() const;
/**
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
*/
- double originY() const;
+ double getOriginY() const;
/**
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
*/
- double resolution() const;
+ double getResolution() const;
/**
* @brief Accessor for the inscribed radius of the robot
* @return The inscribed radius
*/
- double inscribedRadius() const { return inscribed_radius_; }
+ double getInscribedRadius() const { return inscribed_radius_; }
/**
* @brief Accessor for the circumscribed radius of the robot
* @return The circumscribed radius
*/
- double circumscribedRadius() const { return circumscribed_radius_; }
+ double getCircumscribedRadius() const { return circumscribed_radius_; }
/**
* @brief Accessor for the inflation radius of the robot
* @return The inflation radius
*/
- double inflationRadius() const { return inflation_radius_; }
+ double getInflationRadius() const { return inflation_radius_; }
/**
- * @brief Locks the costmap
- */
- void lock() { lock_.lock(); }
-
- /**
- * @brief Unlocks the costmap
- */
- void unlock() { lock_.unlock(); }
-
- /**
* @brief Sets the cost of a convex polygon to a desired value
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
@@ -321,12 +304,13 @@
virtual void updateOrigin(double new_origin_x, double new_origin_y);
/**
- * @brief Check if a cell falls within the circumscribed radius of the robot
+ * @brief Check if a cell falls within the circumscribed radius of the
+ * robot but outside the inscribed radius of the robot
* @param The x coordinate of the cell
* @param The y coordinate of the cell
- * @return True if the cell is inside the circumscribed radius, false otherwise
+ * @return True if the cell is inside the circumscribed radius but outside the inscribed radius, false otherwise
*/
- bool circumscribedCell(unsigned int x, unsigned int y) const;
+ bool isCircumscribedCell(unsigned int x, unsigned int y) const;
/**
* @brief Given a distance... compute a cost
@@ -356,6 +340,7 @@
return circumscribed_cost_lb_;
}
+ protected:
/**
* @brief Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation
* @param index The index of the cell
@@ -388,7 +373,6 @@
}
private:
-
/**
* @brief Insert new obstacles into the cost map
* @param obstacles The point clouds of obstacles to insert into the map
@@ -555,7 +539,6 @@
double weight_;
unsigned char circumscribed_cost_lb_;
std::priority_queue<CellData> inflation_queue_;
- boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
//functors for raytracing actions
class ClearCell {
Modified: pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -63,6 +63,7 @@
// Thread suppport
#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
#include <costmap_2d/rate.h>
@@ -90,12 +91,12 @@
~Costmap2DROS();
/**
- * @brief If you want to manage your own observation buffer you can add it to the costmap
- * <B>(NOTE: The buffer will be deleted on destruction of the costmap...
- * perhaps a boost shared pointer should go here eventually)</B>
- * @param buffer A pointer to your observation buffer
+ * @brief If you want to manage your own observation buffer you can add
+ * it to the costmap. Note, this is somewhat experimental as this feature
+ * has not been used enough to have been proven reliable.
+ * @param buffer A shared pointer to your observation buffer
*/
- void addObservationBuffer(ObservationBuffer* buffer);
+ void addObservationBuffer(const boost::shared_ptr<ObservationBuffer>& buffer);
/**
* @brief Get the observations used to mark space
@@ -112,20 +113,6 @@
bool getClearingObservations(std::vector<Observation>& clearing_observations);
/**
- * @brief A callback to handle buffering LaserScan messages
- * @param message The message returned from a message notifier
- * @param buffer A pointer to the observation buffer to update
- */
- void laserScanCallback(const tf::MessageNotifier<sensor_msgs::LaserScan>::MessagePtr& message, ObservationBuffer* buffer);
-
- /**
- * @brief A callback to handle buffering PointCloud messages
- * @param message The message returned from a message notifier
- * @param buffer A pointer to the observation buffer to update
- */
- void pointCloudCallback(const tf::MessageNotifier<robot_msgs::PointCloud>::MessagePtr& message, ObservationBuffer* buffer);
-
- /**
* @brief Update the underlying costmap with new sensor data.
* If you want to update the map outside of the update loop that runs, you can call this.
*/
@@ -187,64 +174,58 @@
void getCostmapCopy(Costmap2D& costmap);
/**
- * @brief Returns a copy of the underlying unsigned character array <B>(NOTE: THE USER IS RESPONSIBLE FOR DELETION)</B>
- * @return A copy of the underlying unsigned character character array used for the costmap
- */
- unsigned char* getCharMapCopy();
-
- /**
* @brief Returns the x size of the costmap in cells
* @return The x size of the costmap in cells
*/
- unsigned int cellSizeX();
+ unsigned int getSizeInCellsX();
/**
* @brief Returns the y size of the costmap in cells
* @return The y size of the costmap in cells
*/
- unsigned int cellSizeY();
+ unsigned int getSizeInCellsY();
/**
* @brief Returns the resolution of the costmap in meters
* @return The resolution of the costmap in meters
*/
- double resolution();
+ double getResolution();
/**
* @brief Returns the global frame of the costmap
* @return The global frame of the costmap
*/
- std::string globalFrame();
+ std::string getGlobalFrameID();
/**
* @brief Returns the local frame of the costmap
* @return The local frame of the costmap
*/
- std::string baseFrame();
+ std::string getBaseFrameID();
/**
* @brief Returns the inscribed radius of the costmap
* @return The inscribed radius of the costmap
*/
- double inscribedRadius();
+ double getInscribedRadius();
/**
* @brief Returns the circumscribed radius of the costmap
* @return The circumscribed radius of the costmap
*/
- double circumscribedRadius();
+ double getCircumscribedRadius();
/**
* @brief Returns the inflation radius of the costmap
* @return The inflation radius of the costmap
*/
- double inflationRadius();
+ double getInflationRadius();
/**
* @brief Returns the footprint of the robot in the robot_base_frame. To get the footprint in the global_frame use getOrientedFootprint
* @return The footprint of the robot in the robot_base_frame
*/
- std::vector<robot_msgs::Point> robotFootprint();
+ std::vector<robot_msgs::Point> getRobotFootprint();
/**
* @brief Check if the observation buffers for the cost map are current
@@ -253,7 +234,9 @@
bool isCurrent() {return current_;}
/**
- * @brief Subscribes to sensor topics and starts costmap updates
+ * @brief Subscribes to sensor topics if necessary and starts costmap
+ * updates, can be called to restart the costmap after calls to either
+ * stop() or pause()
*/
void start();
@@ -286,6 +269,20 @@
private:
/**
+ * @brief A callback to handle buffering LaserScan messages
+ * @param message The message returned from a message notifier
+ * @param buffer A pointer to the observation buffer to update
+ */
+ void laserScanCallback(const tf::MessageNotifier<sensor_msgs::LaserScan>::MessagePtr& message, const boost::shared_ptr<ObservationBuffer>& buffer);
+
+ /**
+ * @brief A callback to handle buffering PointCloud messages
+ * @param message The message returned from a message notifier
+ * @param buffer A pointer to the observation buffer to update
+ */
+ void pointCloudCallback(const tf::MessageNotifier<robot_msgs::PointCloud>::MessagePtr& message, const boost::shared_ptr<ObservationBuffer>& buffer);
+
+ /**
* @brief The loop that handles updating the costmap
* @param frequency The rate at which to run the loop
*/
@@ -304,18 +301,19 @@
std::string robot_base_frame_; ///< @brief The frame_id of the robot base
boost::thread* map_update_thread_; ///< @brief A thread for updating the map
- std::vector<tf::MessageNotifierBase*> observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
- std::vector<ObservationBuffer*> observation_buffers_; ///< @brief Used to store observations from various sensors
- std::vector<ObservationBuffer*> marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
- std::vector<ObservationBuffer*> clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
+ std::vector<boost::shared_ptr<tf::MessageNotifierBase> > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
+ std::vector<boost::shared_ptr<ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
+ std::vector<boost::shared_ptr<ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
+ std::vector<boost::shared_ptr<ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
bool rolling_window_; ///< @brief Whether or not the costmap should roll with the robot
bool current_; ///< @brief Whether or not all the observation buffers are updating at the desired rate
double transform_tolerance_; // timeout before transform errors
Costmap2DPublisher* costmap_publisher_;
- bool stop_updates_, initialized_;
+ bool stop_updates_, initialized_, stopped_;
bool publish_voxel_;
std::vector<robot_msgs::Point> footprint_spec_;
ros::Publisher voxel_pub_;
+ boost::recursive_mutex lock_;
};
};
Modified: pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation.h
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation.h 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -45,13 +45,35 @@
*/
class Observation {
public:
- // Structors
- Observation() : cloud_(){}
- Observation(robot_msgs::Point& p, robot_msgs::PointCloud cloud): origin_(p), cloud_(cloud) {}
- Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
+ /**
+ * @brief Creates an empty observation
+ */
+ Observation() : cloud_(), raytrace_range_(0.0){}
+ /**
+ * @brief Creates an observation from an origin point and a point cloud
+ * @param origin The origin point of the observation
+ * @param cloud The point cloud of the observation
+ * @param raytrace_range The range out to which an observation should be able to clear via raytracing
+ */
+ Observation(robot_msgs::Point& origin, robot_msgs::PointCloud cloud, double raytrace_range): origin_(origin),
+ cloud_(cloud), raytrace_range_(raytrace_range) {}
+
+ /**
+ * @brief Copy constructor
+ * @param obs The observation to copy
+ */
+ Observation(const Observation& obs): origin_(obs.origin_), cloud_(obs.cloud_), raytrace_range_(obs.raytrace_range_){}
+
+ /**
+ * @brief Creates an observation from a point cloud
+ * @param cloud The point cloud of the observation
+ */
+ Observation(robot_msgs::PointCloud cloud): cloud_(cloud), raytrace_range_(0.0){}
+
robot_msgs::Point origin_;
robot_msgs::PointCloud cloud_;
+ double raytrace_range_;
};
}
Modified: pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation_buffer.h
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation_buffer.h 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/include/costmap_2d/observation_buffer.h 2009-08-04 03:19:26 UTC (rev 20621)
@@ -62,12 +62,13 @@
* @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
* @param min_obstacle_height The minimum height of a hitpoint to be considered legal
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
+ * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
* @param tf A reference to a TransformListener
* @param global_frame The frame to transform PointClouds into
- * @param sensor_frame The frame of the origin of the sensor, special value frame_from_message means use the incoming message frame
+ * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
*/
ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
- double min_obstacle_height, double max_obstacle_height,
+ double min_obstacle_height, double max_obstacle_height, double raytrace_range,
tf::TransformListener& tf, std::string global_frame, std::string sensor_frame);
/**
@@ -120,6 +121,7 @@
std::string topic_name_;
double min_obstacle_height_, max_obstacle_height_;
boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
+ double raytrace_range_;
};
};
#endif
Modified: pkg/trunk/stacks/navigation/costmap_2d/launch_costmap_2d_ros.xml
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/launch_costmap_2d_ros.xml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/launch_costmap_2d_ros.xml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -27,15 +27,13 @@
<param name="costmap/transform_tolerance" value="100.0" />
- <param name="costmap/observation_topics" value="tilt_scan" />
+ <param name="costmap/observation_sources" value="tilt_scan" />
- <param name="costmap/base_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/base_scan/observation_persistence" value="0.0" />
<param name="costmap/base_scan/expected_update_rate" value="0.2" />
<param name="costmap/base_scan/data_type" value="LaserScan" />
<param name="costmap/base_scan/clearing" value="true" />
- <param name="costmap/tilt_scan/sensor_frame" value="frame_from_message" />
<param name="costmap/tilt_scan/observation_persistence" value="10.0" />
<param name="costmap/tilt_scan/expected_update_rate" value="0.2" />
<param name="costmap/tilt_scan/data_type" value="LaserScan" />
Modified: pkg/trunk/stacks/navigation/costmap_2d/mainpage.dox
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/mainpage.dox 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/mainpage.dox 2009-08-04 03:19:26 UTC (rev 20621)
@@ -57,7 +57,7 @@
<br><br>
<li><b>~inflation_radius</b>, <i>double</i> <br>The radius to which the map inflates obstacle cost values</li>
<br><br>
-<li><b>~observation_topics</b>, <i>string</i> <br>A list of topics to subscribe to separated by spaces</li>
+<li><b>~observation_sources</b>, <i>string</i> <br>A list of topics to subscribe to separated by spaces</li>
<br><br>
<li>
<b>Each topic can also have parameters set on it</b><br><br>
Modified: pkg/trunk/stacks/navigation/costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/manifest.xml 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/manifest.xml 2009-08-04 03:19:26 UTC (rev 20621)
@@ -10,7 +10,7 @@
</description>
<author>Eitan Marder-Eppstein</author>
<license>BSD</license>
-<review status="proposal cleared" notes=""/>
+<review status="api cleared" notes=""/>
<url>http://pr.willowgarage.com/wiki/costmap_2d</url>
<depend package="rosconsole"/>
<depend package="roscpp" />
Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -217,12 +217,6 @@
return (unsigned int) cells_dist;
}
- unsigned char* Costmap2D::getCharMapCopy() const {
- unsigned char* map_copy = new unsigned char[size_x_ * size_y_];
- memcpy(map_copy, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
- return map_copy;
- }
-
const unsigned char* Costmap2D::getCharMap() const {
return costmap_;
}
@@ -273,8 +267,8 @@
start_point_y = max(origin_y_, start_point_y);
//check end bounds
- end_point_x = min(origin_x_ + metersSizeX(), end_point_x);
- end_point_y = min(origin_y_ + metersSizeY(), end_point_y);
+ end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x);
+ end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y);
unsigned int start_x, start_y, end_x, end_y;
@@ -365,10 +359,13 @@
const PointCloud& cloud =obs.cloud_;
+
for(unsigned int i = 0; i < cloud.pts.size(); ++i){
//if the obstacle is too high or too far away from the robot we won't add it
- if(cloud.pts[i].z > max_obstacle_height_)
+ if(cloud.pts[i].z > max_obstacle_height_){
+ ROS_DEBUG("The point is too high");
continue;
+ }
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (cloud.pts[i].x - obs.origin_.x) * (cloud.pts[i].x - obs.origin_.x)
@@ -376,13 +373,17 @@
+ (cloud.pts[i].z - obs.origin_.z) * (cloud.pts[i].z - obs.origin_.z);
//if the point is far enough away... we won't consider it
- if(sq_dist >= sq_obstacle_range_)
+ if(sq_dist >= sq_obstacle_range_){
+ ROS_DEBUG("The point is too far away");
continue;
+ }
//now we need to compute the map coordinates for the observation
unsigned int mx, my;
- if(!worldToMap(cloud.pts[i].x, cloud.pts[i].y, mx, my))
+ if(!worldToMap(cloud.pts[i].x, cloud.pts[i].y, mx, my)){
+ ROS_DEBUG("Computing map coords failed");
continue;
+ }
unsigned int index = getIndex(mx, my);
@@ -439,8 +440,8 @@
return;
//we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
- double map_end_x = origin_x_ + metersSizeX();
- double map_end_y = origin_y_ + metersSizeY();
+ double map_end_x = origin_x_ + getSizeInMetersX();
+ double map_end_y = origin_y_ + getSizeInMetersY();
//for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
for(unsigned int i = 0; i < cloud.pts.size(); ++i){
@@ -483,7 +484,7 @@
if(!worldToMap(wx, wy, x1, y1))
continue;
- unsigned int cell_raytrace_range = cellDistance(raytrace_range_);
+ unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
//and finally... we can execute our trace to clear obstacles along that line
raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range);
@@ -506,8 +507,8 @@
start_x = max(origin_x_, start_x);
start_y = max(origin_y_, start_y);
- end_x = min(origin_x_ + metersSizeX(), end_x);
- end_y = min(origin_y_ + metersSizeY(), end_y);
+ end_x = min(origin_x_ + getSizeInMetersX(), end_x);
+ end_y = min(origin_y_ + getSizeInMetersY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
@@ -551,8 +552,8 @@
start_x = max(origin_x_, start_x);
start_y = max(origin_y_, start_y);
- end_x = min(origin_x_ + metersSizeX(), end_x);
- end_y = min(origin_y_ + metersSizeY(), end_y);
+ end_x = min(origin_x_ + getSizeInMetersX(), end_x);
+ end_y = min(origin_y_ + getSizeInMetersY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
@@ -749,35 +750,35 @@
}
}
- unsigned int Costmap2D::cellSizeX() const{
+ unsigned int Costmap2D::getSizeInCellsX() const{
return size_x_;
}
- unsigned int Costmap2D::cellSizeY() const{
+ unsigned int Costmap2D::getSizeInCellsY() const{
return size_y_;
}
- double Costmap2D::metersSizeX() const{
+ double Costmap2D::getSizeInMetersX() const{
return (size_x_ - 1 + 0.5) * resolution_;
}
- double Costmap2D::metersSizeY() const{
+ double Costmap2D::getSizeInMetersY() const{
return (size_y_ - 1 + 0.5) * resolution_;
}
- double Costmap2D::originX() const{
+ double Costmap2D::getOriginX() const{
return origin_x_;
}
- double Costmap2D::originY() const{
+ double Costmap2D::getOriginY() const{
return origin_y_;
}
- double Costmap2D::resolution() const{
+ double Costmap2D::getResolution() const{
return resolution_;
}
- bool Costmap2D::circumscribedCell(unsigned int x, unsigned int y) const {
+ bool Costmap2D::isCircumscribedCell(unsigned int x, unsigned int y) const {
unsigned char cost = getCost(x, y);
if(cost < INSCRIBED_INFLATED_OBSTACLE && cost >= circumscribed_cost_lb_)
return true;
Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -40,8 +40,8 @@
Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle& ros_node, double publish_frequency, std::string global_frame)
: ros_node_(ros_node), global_frame_(global_frame),
visualizer_thread_(NULL), active_(false), new_data_(false){
- raw_obs_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("~raw_obstacles", 1);
- inf_obs_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("~inflated_obstacles", 1);
+ raw_obs_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("raw_obstacles", 1);
+ inf_obs_pub_ = ros_node_.advertise<visualization_msgs::Polyline>("inflated_obstacles", 1);
visualizer_thread_ = new boost::thread(boost::bind(&Costmap2DPublisher::mapPublishLoop, this, publish_frequency));
}
@@ -73,8 +73,8 @@
void Costmap2DPublisher::updateCostmapData(const Costmap2D& costmap){
std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles;
- for(unsigned int i = 0; i < costmap.cellSizeX(); i++){
- for(unsigned int j = 0; j < costmap.cellSizeY(); j++){
+ for(unsigned int i = 0; i < costmap.getSizeInCellsX(); i++){
+ for(unsigned int j = 0; j < costmap.getSizeInCellsY(); j++){
double wx, wy;
costmap.mapToWorld(i, j, wx, wy);
std::pair<double, double> p(wx, wy);
Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp 2009-08-04 03:18:55 UTC (rev 20620)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp 2009-08-04 03:19:26 UTC (rev 20621)
@@ -45,13 +45,13 @@
return x < 0.0 ? -1.0 : 1.0;
}
- Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) : ros_node_(name),
- tf_(tf), costmap_(NULL), map_update_thread_(NULL), costmap_publisher_(NULL), stop_updates_(false), initialized_(true) {
+ Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) : ros_node_("~/" + name),
+ tf_(tf), costmap_(NULL), map_update_thread_(NULL), costmap_publisher_(NULL), stop_updates_(false), initialized_(true), stopped_(false) {
std::string map_type;
- ros_node_.param("~map_type", map_type, std::string("costmap"));
+ ros_node_.param("map_type", map_type, std::string("costmap"));
- ros_node_.param("~publish_voxel_map", publish_voxel_, false);
+ ros_node_.param("publish_voxel_map", publish_voxel_, false);
if(publish_voxel_ && map_type == "voxel")
voxel_pub_ = ros_node_.advertise<costmap_2d::VoxelGrid>("~/voxel_grid", 1);
@@ -60,48 +60,67 @@
std::string topics_string;
//get the topics that we'll subscribe to from the parameter server
- ros_node_.param("~observation_topics", topics_string, std::string(""));
+ ros_node_.param("observation_sources", topics_string, std::string(""));
ROS_INFO("Subscribed to Topics: %s", topics_string.c_str());
- ros_node_.param("~global_frame", global_frame_, std::string("/map"));
- ros_node_.param("~robot_base_frame", robot_base_frame_, std::string("base_link"));
+ ros_node_.param("global_frame", global_frame_, std::string("/map"));
+ ros_node_.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
ros::Time last_error = ros::Time::now();
+ std::string tf_error;
//we need to make sure that the transform between the robot base frame and the global frame is available
- while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1))){
+ while(!tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01), &tf_error)){
ros::spinOnce();
if(last_error + ros::Duration(5.0) < ros::Time::now()){
- ROS_ERROR("Waiting on transform from %s to %s to become available before running costmap", robot_base_frame_.c_str(), global_frame_.c_str());
+ ROS_ERROR("Waiting on transform from %s to %s to become available before running costmap, tf error: %s",
+ robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = ros::Time::now();
}
}
- ros_node_.param("~transform_tolerance", transform_tolerance_, 0.2);
+ ros_node_.param("transform_tolerance", transform_tolerance_, 0.2);
//now we need to split the topics based on whitespace which we can use a stringstream for
std::stringstream ss(topics_string);
- std::string topic;
- while(ss >> topic){
+ double raytrace_range = 3.0;
+
+ std::string source;
+ while(ss >> source){
+ ros::NodeHandle source_node(ros_node_, source);
//get the parameters for the specific topic
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
- std::string sensor_frame, data_type;
- ros_node_.param("~" + topic + "/sensor_frame", sensor_frame, std::string("frame_from_message"));
- ros_node_.param("~" + topic + "/observation_persistence", observation_keep_time, 0.0);
- ros_node_.param("~" + topic + "/expected_update_rate", expected_update_rate, 0.0);
- ros_node_.param("~" + topic + "/data_type", data_type, std::string("PointCloud"));
- ros_node_.param("~" + topic + "/min_obstacle_height", min_obstacle_height, 0.05);
- ros_node_.param("~" + topic + "/max_obstacle_height", max_obstacle_height, 2.0);
+ std::string topic, sensor_frame, data_type;
+ source_node.param("topic", topic, source);
+ source_node.param("sensor_frame", sensor_frame, std::string(""));
+ source_node.param("observation_persistence", observation_keep_time, 0.0);
+ source_node.param("expected_update_rate", expected_update_rate, 0.0);
+ source_node.param("data_type", data_type, std::string("PointCloud"));
+ source_node.param("min_obstacle_height", min_obstacle_height, 0.05);
+ source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
ROS_ASSERT_MSG(data_type == "PointCloud" || data_type == "LaserScan", "Only topics that use point clouds or laser scans are currently supported");
bool clearing, marking;
- ros_node_.param("~" + topic + "/clearing", clearing, false);
- ros_node_.param("~" + topic + "/marking", marking, true);
+ source_node.param("clearing", clearing, false);
+ source_node.param("marking", marking, true);
+ std::string raytrace_range_param_name;
+ double source_raytrace_range;
+ if(!source_node.searchParam("raytrace_range", raytrace_range_param_name))
+ source_raytrace_range = 3.0;
+ else
+ source_node.param(raytrace_range_param_name, source_raytrace_range, 3.0);
+
+ //keep track of the maximum raytrace range for the costmap to be able to inflate efficiently
+ raytrace_range = std::max(raytrace_range, source_raytrace_range);
+
+ ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), sensor_frame.c_str());
+
//create an observation buffer
- observation_buffers_.push_back(new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, tf_, global_frame_, sensor_frame));
+ observation_buffers_.push_back(boost::shared_ptr<ObservationBuffer>(new ObservationBuffer(topic, observation_keep_time,
+ expected_update_rate, min_obstacle_height, max_obstacle_height, source_raytrace_range, tf_, global_frame_, sensor_frame)));
//check if we'll add this buffer to our marking observation buffers
if(marking)
@@ -111,20 +130,21 @@
if(clearing)
clearing_buffers_.push_back(observation_buffers_.back());
- ROS_DEBUG("Created an observation buffer for topic %s, expected update rate: %.2f, observation persistence: %.2f", topic.c_str(), expected_update_rate, observation_keep_time);
+ ROS_DEBUG("Created an observation buffer for source %s, topic %s, global frame: %s, expected update rate: %.2f, observation persistence: %.2f",
+ source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
//create a callback for the topic
if(data_type == "LaserScan"){
- observation_notifiers_.push_back(new tf::MessageNotifier<sensor_msgs::LaserScan>(tf_,
- boost::bind(&Costmap2DROS::laserScanCallback, this, _1, observation_buffers_.back()), topic, global_frame_, 50));
+ observation_notifiers_.push_back(boost::shared_ptr<tf::MessageNotifierBase>(new tf::MessageNotifier<sensor_msg...
[truncated message content] |
