Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13211] pkg/trunk
|
From: <ei...@us...> - 2009-04-01 02:23:32
|
Revision: 13211
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13211&view=rev
Author: eitanme
Date: 2009-04-01 01:38:24 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r20159@cib: eitan | 2009-03-26 19:24:01 -0700
Tested all parts of costmap functionality successfully... still need to wrap it somehow to pass the old unit tests
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Added Paths:
-----------
pkg/trunk/world_models/new_costmap/src/costmap_test.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20146
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20159
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:38:10 UTC (rev 13210)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:38:24 UTC (rev 13211)
@@ -278,10 +278,10 @@
* @param y0 The starting y coordinate
* @param x1 The ending x coordinate
* @param y1 The ending y coordinate
- * @return
+ * @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint
*/
template <class ActionType>
- inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1){
+ inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length){
int dx = x1 - x0;
int dy = y1 - y0;
@@ -293,16 +293,20 @@
unsigned int offset = y0 * size_x_ + x0;
+ //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
+ double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
+ double scale = std::min(1.0, max_length / dist);
+
//if x is dominant
if(abs_dx >= abs_dy){
int error_y = abs_dx / 2;
- bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset);
+ bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}
//otherwise y is dominant
int error_x = abs_dy / 2;
- bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset);
+ bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
@@ -310,8 +314,9 @@
* @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
*/
template <class ActionType>
- inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
- for(unsigned int i = 0; i < abs_da; ++i){
+ inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b,
+ unsigned int offset, unsigned int max_length){
+ for(unsigned int i = 0; i < std::min(max_length, abs_da); ++i){
at(offset);
offset += offset_a;
error_b += abs_db;
@@ -412,6 +417,16 @@
private:
unsigned char* cost_map_;
};
+
+ class MarkCell {
+ public:
+ MarkCell(unsigned char* cost_map) : cost_map_(cost_map) {}
+ inline void operator()(unsigned int offset){
+ cost_map_[offset] = LETHAL_OBSTACLE;
+ }
+ private:
+ unsigned char* cost_map_;
+ };
};
};
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:38:10 UTC (rev 13210)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:38:24 UTC (rev 13211)
@@ -144,8 +144,11 @@
start_point_y = max(origin_y_, start_point_y);
unsigned int start_x, start_y, end_x, end_y;
- worldToMap(start_point_x, start_point_y, start_x, start_y);
+ //check for legality just in case
+ if(!worldToMap(start_point_x, start_point_y, start_x, start_y))
+ return;
+
//we'll do our own bounds checking for the end of the window
worldToMapNoBounds(start_point_x + w_size_x, start_point_y + w_size_y, end_x, end_y);
@@ -278,9 +281,6 @@
}
void Costmap2D::raytraceFreespace(const Observation& clearing_observation){
- //pre-comput the squared raytrace range... saves a sqrt in an inner loop
- double sq_raytrace_range = raytrace_range_ * raytrace_range_;
-
//create the functor that we'll use to clear cells from the costmap
ClearCell clearer(cost_map_);
@@ -302,50 +302,46 @@
double wx = cloud.pts[i].x;
double wy = cloud.pts[i].y;
- //we want to check that we scale the vector so that we only trace to the desired distance
- double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
- double scaling_fact = sq_raytrace_range / sq_distance;
- scaling_fact = min(1.0, scaling_fact);
-
+ //now we also need to make sure that the enpoint we're raytracing
+ //to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
- wx = ox + scaling_fact * a;
- wy = oy + scaling_fact * b;
-
- //now we also need to make sure that the enpoint we're raytracing
- //to isn't off the costmap and scale if necessary
-
//the minimum value to raytrace from is the origin
if(wx < origin_x_){
double t = (origin_x_ - ox) / a;
- wx = ox + a * t;
+ wx = origin_x_;
wy = oy + b * t;
}
if(wy < origin_y_){
double t = (origin_y_ - oy) / b;
wx = ox + a * t;
- wy = oy + b * t;
+ wy = origin_y_;
}
//the maximum value to raytrace to is the end of the map
if(wx > map_end_x){
double t = (map_end_x - ox) / a;
- wx = ox + a * t;
+ wx = map_end_x;
wy = oy + b * t;
}
if(wy > map_end_y){
double t = (map_end_y - oy) / b;
wx = ox + a * t;
- wy = oy + b * t;
+ wy = map_end_y;
}
//now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
- worldToMap(wx, wy, x1, y1);
+ //check for legality just in case
+ if(!worldToMap(wx, wy, x1, y1))
+ continue;
+
+ unsigned int cell_raytrace_range = cellDistance(raytrace_range_);
+
//and finally... we can execute our trace to clear obstacles along that line
- raytraceLine(clearer, x0, y0, x1, y1);
+ raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range);
}
}
@@ -371,9 +367,11 @@
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
- worldToMap(start_x, start_y, map_sx, map_sy);
- worldToMap(end_x, end_y, map_ex, map_ey);
+ //check for legality just in case
+ if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
+ return;
+
//we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &cost_map_[index];
Added: pkg/trunk/world_models/new_costmap/src/costmap_test.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_test.cpp (rev 0)
+++ pkg/trunk/world_models/new_costmap/src/costmap_test.cpp 2009-04-01 01:38:24 UTC (rev 13211)
@@ -0,0 +1,308 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <ros/node.h>
+#include <ros/console.h>
+#include <new_costmap/costmap_2d.h>
+#include <costmap_2d/costmap_2d.h>
+#include <robot_srvs/StaticMap.h>
+#include <robot_msgs/Polyline2D.h>
+#include <map>
+#include <vector>
+
+#include <tf/transform_datatypes.h>
+#include <tf/message_notifier.h>
+#include <tf/transform_listener.h>
+
+#include <laser_scan/LaserScan.h>
+#include <laser_scan/laser_scan.h>
+
+#include <robot_msgs/PointCloud.h>
+
+// Thread suppport
+#include <boost/thread.hpp>
+
+using namespace costmap_2d;
+using namespace tf;
+using namespace robot_msgs;
+
+class CostmapTester {
+ public:
+ CostmapTester(ros::Node& ros_node) : ros_node_(ros_node), base_scan_notifier_(NULL), tf_(ros_node, true, ros::Duration(10)), observations_(1), global_frame_("map"), freq_(5) {
+ ros_node.advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
+ ros_node.advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+
+ base_scan_notifier_ = new MessageNotifier<laser_scan::LaserScan>(&tf_, &ros_node,
+ boost::bind(&CostmapTester::baseScanCallback, this, _1),
+ "base_scan", global_frame_, 50);
+
+ robot_srvs::StaticMap::Request map_req;
+ robot_srvs::StaticMap::Response map_resp;
+ ROS_INFO("Requesting the map...\n");
+ while(!ros::service::call("static_map", map_req, map_resp))
+ {
+ ROS_INFO("Request failed; trying again...\n");
+ usleep(1000000);
+ }
+ ROS_INFO("Received a %d X %d map at %f m/pix\n",
+ map_resp.map.width, map_resp.map.height, map_resp.map.resolution);
+
+ // We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
+ // our planner and controller do not reason about the no obstacle case
+ std::vector<unsigned char> input_data;
+ unsigned int numCells = map_resp.map.width * map_resp.map.height;
+ for(unsigned int i = 0; i < numCells; i++){
+ input_data.push_back((unsigned char) map_resp.map.data[i]);
+ }
+
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+ new_costmap_ = new Costmap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
+ map_resp.map.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
+ 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_INFO("New map construction time: %.9f", t_diff);
+
+ gettimeofday(&start, NULL);
+ old_costmap_ = new CostMap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
+ input_data , map_resp.map.resolution,
+ 100, 2.0, 15, 25,
+ 0.55, 0.46, 0.325, 1.0,
+ 10.0, 10.0, 10.0);
+ 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_INFO("Old map construction time: %.9f", t_diff);
+
+ //create a separate thread to publish cost data to the visualizer
+ visualizer_thread_ = new boost::thread(boost::bind(&CostmapTester::publishCostMap, this));
+ window_reset_thread_ = new boost::thread(boost::bind(&CostmapTester::resetWindow, this));
+
+ }
+
+ ~CostmapTester(){
+ delete new_costmap_;
+ delete old_costmap_;
+ delete base_scan_notifier_;
+ delete visualizer_thread_;
+ delete window_reset_thread_;
+ }
+
+ void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message){
+ //project the laser into a point cloud
+ PointCloud base_cloud;
+ base_cloud.header = message->header;
+ //we want all values... even those out of range
+ projector_.projectLaser(*message, base_cloud, -1.0, true);
+ Stamped<btVector3> global_origin;
+
+ lock_.lock();
+ //we know the transform is available from the laser frame to the global frame
+ try{
+ //transform the origin for the sensor
+ Stamped<btVector3> local_origin(btVector3(0, 0, 0), base_cloud.header.stamp, base_cloud.header.frame_id);
+ tf_.transformPoint(global_frame_, local_origin, global_origin);
+ observations_[0].origin_.x = global_origin.getX();
+ observations_[0].origin_.y = global_origin.getY();
+ observations_[0].origin_.z = global_origin.getZ();
+
+ //transform the point cloud
+ tf_.transformPointCloud(global_frame_, base_cloud, observations_[0].cloud_);
+ }
+ catch(tf::TransformException& ex){
+ ROS_ERROR("TF Exception that should never happen %s", ex.what());
+ return;
+ }
+ lock_.unlock();
+ }
+
+ void spin(){
+ while(ros_node_.ok()){
+ updateMap();
+ usleep(1e6/freq_);
+ }
+ }
+
+ void updateMap(){
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ global_pose.setIdentity();
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time();
+ try{
+ tf_.transformPose(global_frame_, robot_pose, global_pose);
+ }
+ catch(tf::LookupException& ex) {
+ ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException& ex) {
+ ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ }
+
+ double wx = global_pose.getOrigin().x();
+ double wy = global_pose.getOrigin().y();
+
+ //in the real world... make a deep copy... but for testing I'm lazy so I'll lock around the updates
+ lock_.lock();
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+ new_costmap_->updateWorld(wx, wy, observations_, observations_);
+ 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_INFO("Map update time: %.9f", t_diff);
+ lock_.unlock();
+ }
+
+ void resetWindow(){
+ while(ros_node_.ok()){
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ global_pose.setIdentity();
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time();
+ try{
+ tf_.transformPose(global_frame_, robot_pose, global_pose);
+ }
+ catch(tf::LookupException& ex) {
+ ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException& ex) {
+ ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ }
+
+ double wx = global_pose.getOrigin().x();
+ double wy = global_pose.getOrigin().y();
+ lock_.lock();
+ ROS_INFO("Resetting map outside window");
+ new_costmap_->resetMapOutsideWindow(wx, wy, 5.0, 5.0);
+ lock_.unlock();
+
+ usleep(1e6/0.2);
+ }
+ }
+
+ void publishCostMap(){
+ while(ros_node_.ok()){
+ ROS_INFO("publishing map");
+ lock_.lock();
+ std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles;
+ for(unsigned int i = 0; i<new_costmap_->cellSizeX(); i++){
+ for(unsigned int j = 0; j<new_costmap_->cellSizeY();j++){
+ double wx, wy;
+ new_costmap_->mapToWorld(i, j, wx, wy);
+ std::pair<double, double> p(wx, wy);
+
+ if(new_costmap_->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+ raw_obstacles.push_back(p);
+ else if(new_costmap_->getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+ inflated_obstacles.push_back(p);
+ }
+ }
+ lock_.unlock();
+
+ // First publish raw obstacles in red
+ robot_msgs::Polyline2D obstacle_msg;
+ obstacle_msg.header.frame_id = global_frame_;
+ unsigned int pointCount = raw_obstacles.size();
+ obstacle_msg.set_points_size(pointCount);
+ obstacle_msg.color.a = 0.0;
+ obstacle_msg.color.r = 1.0;
+ obstacle_msg.color.b = 0.0;
+ obstacle_msg.color.g = 0.0;
+
+ for(unsigned int i=0;i<pointCount;i++){
+ obstacle_msg.points[i].x = raw_obstacles[i].first;
+ obstacle_msg.points[i].y = raw_obstacles[i].second;
+ }
+
+ ros::Node::instance()->publish("raw_obstacles", obstacle_msg);
+
+ // Now do inflated obstacles in blue
+ pointCount = inflated_obstacles.size();
+ obstacle_msg.set_points_size(pointCount);
+ obstacle_msg.color.a = 0.0;
+ obstacle_msg.color.r = 0.0;
+ obstacle_msg.color.b = 1.0;
+ obstacle_msg.color.g = 0.0;
+
+ for(unsigned int i=0;i<pointCount;i++){
+ obstacle_msg.points[i].x = inflated_obstacles[i].first;
+ obstacle_msg.points[i].y = inflated_obstacles[i].second;
+ }
+
+ ros::Node::instance()->publish("inflated_obstacles", obstacle_msg);
+ usleep(1e6/2.0);
+ }
+ }
+
+ ros::Node& ros_node_;
+ tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
+ tf::TransformListener tf_; ///< @brief Used for transforming point clouds
+ laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
+ boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
+ std::vector<Observation> observations_;
+ Costmap2D* new_costmap_;
+ CostMap2D* old_costmap_;
+ std::string global_frame_;
+ double freq_;
+ boost::thread *visualizer_thread_;
+ boost::thread *window_reset_thread_;
+
+};
+
+int main(int argc, char** argv){
+ ros::init(argc, argv);
+ ros::Node ros_node("new_costmap_tester");
+ CostmapTester tester(ros_node);
+ tester.spin();
+
+ return(0);
+
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
