mapl.cc
Go to the documentation of this file.
00001 /*
00002  * ROS node for ART map lanes.
00003  *
00004  *  Copyright (C) 2005 Austin Robot Technology
00005  *  Copyright (C) 2010 Austin Robot Technology
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: mapl.cc 2125 2012-02-28 00:34:05Z jack.oquin $
00010  */
00011 
00012 #include <unistd.h>
00013 #include <string.h>
00014 #include <iostream>
00015 
00016 #include <ros/ros.h>
00017 #include <tf/tf.h>
00018 
00019 #include <art_msgs/ArtHertz.h>
00020 #include <sensor_msgs/PointCloud.h>
00021 #include <nav_msgs/Odometry.h>
00022 #include <visualization_msgs/MarkerArray.h>
00023 
00024 #include <art_msgs/ArtLanes.h>
00025 #include <art_map/Graph.h>
00026 #include <art_map/MapLanes.h>
00027 #include <art_map/RNDF.h>
00028 
00029 #include <art_msgs/ArtVehicle.h>
00030 
00055 class MapLanesDriver
00056 {
00057 public:
00058     
00059   // Constructor
00060   MapLanesDriver();
00061   ~MapLanesDriver()
00062     {
00063       delete map_;
00064       if (graph_)
00065         delete graph_;
00066     };
00067 
00068   bool buildRoadMap(void);
00069   int  Setup(ros::NodeHandle node);
00070   int  Shutdown(void);
00071   void Spin(void);
00072 
00073 private:
00074 
00075   void markCar();
00076   void processOdom(const nav_msgs::Odometry::ConstPtr &odomIn);
00077   void publishGlobalMap(void);
00078   void publishLocalMap(void);
00079   void publishMapCloud(ros::Publisher &pub,
00080                        const art_msgs::ArtLanes &lane_data);
00081   void publishMapMarks(ros::Publisher &pub,
00082                        const std::string &map_name,
00083                        ros::Duration life,
00084                        const art_msgs::ArtLanes &lane_data);
00085 
00086   // parameters:
00087   double range_;                
00088   double poly_size_;            
00089   std::string rndf_name_;       
00090   std::string frame_id_;        
00091 
00092   // topics and messages
00093   ros::Subscriber odom_topic_;       // odometry topic
00094   nav_msgs::Odometry odom_msg_;      // last Odometry message received
00095 
00096   ros::Publisher roadmap_global_;       // global road map publisher
00097   ros::Publisher roadmap_local_;        // local road map publisher
00098   ros::Publisher mapmarks_;             // rviz visualization markers
00099   ros::Publisher car_image_;            // rviz marker for 3D image of car
00100 
00101   ros::Publisher roadmap_cloud_;        // local road map point cloud
00102 
00103   // this vector is only used while publishMapMarks() is running
00104   // we define it here to avoid memory allocation on every cycle
00105   sensor_msgs::PointCloud cloud_msg_;
00106 
00107   // this vector is only used while publishMapMarks() is running
00108   // we define it here to avoid memory allocation on every cycle
00109   visualization_msgs::MarkerArray marks_msg_;
00110 
00111   Graph *graph_;                  
00112   MapLanes* map_;                 
00113   bool initial_position_;         
00114 };
00115 
00117 MapLanesDriver::MapLanesDriver(void)
00118 {
00119   initial_position_ = false;
00120 
00121   // use private node handle to get parameters
00122   ros::NodeHandle nh("~");
00123 
00124   frame_id_ = "/map";
00125   if (nh.getParam("frame_id", frame_id_))
00126     {
00127       ROS_INFO_STREAM("map frame ID = " << frame_id_);
00128     }
00129 
00130   nh.param("range", range_, 80.0);
00131   ROS_INFO("range to publish = %.0f meters", range_);
00132 
00133   nh.param("poly_size", poly_size_, MIN_POLY_SIZE);
00134   ROS_INFO("polygon size = %.0f meters", poly_size_);
00135 
00136   rndf_name_ = "";
00137   std::string rndf_param;
00138   if (nh.searchParam("rndf", rndf_param))
00139     {
00140       nh.param(rndf_param, rndf_name_, std::string(""));
00141       ROS_INFO_STREAM("RNDF: " << rndf_name_);
00142     }
00143   else
00144     {
00145       ROS_ERROR("RNDF not defined");
00146     }
00147 
00148   // create the MapLanes class
00149   map_ = new MapLanes(range_);
00150   graph_ = NULL;
00151 }
00152 
00158 void MapLanesDriver::markCar()
00159 {
00160   using art_msgs::ArtVehicle;
00161   visualization_msgs::Marker car;
00162   car.header.stamp = ros::Time();       // zero time means "always"
00163   car.header.frame_id = "/vehicle";
00164   car.frame_locked = true;
00165 
00166   // publish polygon centers
00167   car.ns = "Marvin";
00168   car.id = (int32_t) 0;
00169   car.type = visualization_msgs::Marker::MESH_RESOURCE;
00170   car.action = visualization_msgs::Marker::ADD;
00171 
00172   // Pose is same as the /vehicle frame, except the image center is
00173   // not the vehicle origin and it is rotated 90 degrees to the right.
00174   car.pose.position.x = ArtVehicle::halflength + ArtVehicle::rear_bumper_px;
00175   car.pose.position.z = ArtVehicle::halfheight;
00176   car.pose.orientation = tf::createQuaternionMsgFromYaw(M_PI/2.0);
00177 
00178   // mesh vehicle proportions are about right, but its size is very small
00179   car.scale.x = 2.4;
00180   car.scale.y = 2.4;
00181   car.scale.z = 2.4;
00182 
00183   // make car white, semitransparent
00184   car.color.r = 1.0;
00185   car.color.g = 1.0;
00186   car.color.b = 1.0;
00187   car.color.a = 0.5;
00188   car.mesh_resource = "package://art_map/etc/car.dae";
00189   car.mesh_use_embedded_materials = false;
00190 
00191   // indefinite duration
00192   car.lifetime = ros::Duration();
00193 
00194   // publish this as a persistent topic
00195   car_image_.publish(car);
00196 }
00197 
00199 int MapLanesDriver::Setup(ros::NodeHandle node)
00200 {   
00201   static int qDepth = 1;
00202   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00203 
00204   odom_topic_ = node.subscribe("odom", qDepth, &MapLanesDriver::processOdom,
00205                                this, noDelay);
00206 
00207   // Local road map publisher
00208   roadmap_local_ =
00209     node.advertise<art_msgs::ArtLanes>("roadmap_local", qDepth);
00210 
00211   // Local road map point cloud publisher
00212   cloud_msg_.channels.clear();
00213   roadmap_cloud_ =
00214     node.advertise<sensor_msgs::PointCloud>("roadmap_cloud", qDepth);
00215 
00216   // Use latched publisher for global road map and visualization topics
00217   roadmap_global_ =
00218     node.advertise<art_msgs::ArtLanes>("roadmap_global", 1, true);
00219   mapmarks_ = node.advertise <visualization_msgs::MarkerArray>
00220     ("visualization_marker_array", 1);
00221 
00222   // add marker for odometry pose of car
00223   car_image_ =
00224     node.advertise <visualization_msgs::Marker>("visualization_marker",
00225                                                 1, true);
00226   markCar();                            // publish persistent marker
00227 
00228   return 0;
00229 }
00230 
00231 
00233 int MapLanesDriver::Shutdown()
00234 {
00235   // Stop and join the driver thread
00236   ROS_INFO("shutting down maplanes");
00237   return 0;
00238 }
00239 
00241 void MapLanesDriver::processOdom(const nav_msgs::Odometry::ConstPtr &odomIn)
00242 {
00243   odom_msg_ = *odomIn;
00244   if (initial_position_ == false)
00245     {
00246       ROS_INFO("initial odometry received");
00247       initial_position_ = true;         // have position data now
00248     }
00249 }
00250 
00259 void MapLanesDriver::publishMapCloud(ros::Publisher &pub,
00260                                      const art_msgs::ArtLanes &lane_data)
00261 {
00262   if (pub.getNumSubscribers() == 0)     // no subscribers?
00263     return;
00264 
00265   // clear message array, this is a class variable to avoid memory
00266   // allocation and deallocation on every cycle
00267   cloud_msg_.points.resize(3 * lane_data.polygons.size());
00268   cloud_msg_.header.frame_id = frame_id_;
00269   cloud_msg_.header.stamp = ros::Time::now();
00270 
00271   int top_left = art_msgs::ArtQuadrilateral::top_left;
00272   int top_right = art_msgs::ArtQuadrilateral::top_right;
00273 
00274   for (uint32_t i = 0; i < lane_data.polygons.size(); ++i)
00275     {
00276       cloud_msg_.points[3*i].x =
00277         lane_data.polygons[i].midpoint.x;
00278       cloud_msg_.points[3*i].y =
00279         lane_data.polygons[i].midpoint.y;
00280       cloud_msg_.points[3*i].z =
00281         lane_data.polygons[i].midpoint.z;
00282       cloud_msg_.points[3*i+1] =
00283         lane_data.polygons[i].poly.points[top_left];
00284       cloud_msg_.points[3*i+2] =
00285         lane_data.polygons[i].poly.points[top_right];
00286     }
00287 
00288   pub.publish(cloud_msg_);
00289 }
00290 
00304 void MapLanesDriver::publishMapMarks(ros::Publisher &pub,
00305                                      const std::string &map_name,
00306                                      ros::Duration life,
00307                                      const art_msgs::ArtLanes &lane_data)
00308 {
00309   if (pub.getNumSubscribers() == 0)     // no subscribers?
00310     return;
00311 
00312   std_msgs::ColorRGBA green;            // green map markers
00313   green.r = 0.0;
00314   green.g = 1.0;
00315   green.b = 0.0;
00316   green.a = 1.0;
00317   ros::Time now = ros::Time::now();
00318 
00319   // clear message array, this is a class variable to avoid memory
00320   // allocation and deallocation on every cycle
00321   marks_msg_.markers.clear();
00322 
00323   for (uint32_t i = 0; i < lane_data.polygons.size(); ++i)
00324     {
00325 #if 0
00326       visualization_msgs::Marker mark;
00327       mark.header.stamp = now;
00328       mark.header.frame_id = frame_id_;
00329 
00330       // publish polygon centers
00331       mark.ns = "polygons_" + map_name;
00332       mark.id = (int32_t) i;
00333       mark.type = visualization_msgs::Marker::ARROW;
00334       mark.action = visualization_msgs::Marker::ADD;
00335 
00336       mark.pose.position = lane_data.polygons[i].midpoint;
00337       mark.pose.orientation = 
00338         tf::createQuaternionMsgFromYaw(lane_data.polygons[i].heading);
00339 
00340       mark.scale.x = 1.0;
00341       mark.scale.y = 1.0;
00342       mark.scale.z = 1.0;
00343       mark.color = green;
00344       mark.lifetime = life;
00345 
00346       // Add this polygon to the vector of markers to publish
00347       marks_msg_.markers.push_back(mark);
00348 #endif
00349       if (!lane_data.polygons[i].is_transition)
00350         {
00351           visualization_msgs::Marker lane;
00352           lane.header.stamp = now;
00353           lane.header.frame_id = frame_id_;
00354 
00355           // publish lane boundaries (experimental)
00356           //
00357           // It is almost certainly more efficient for rviz rendering
00358           // to collect the right and left lane boundaries for each
00359           // lane as two strips, then publish them as separate
00360           // LINE_STRIP markers. This LINE_LIST version is an
00361           // experiment to see how it looks (pretty good).
00362           lane.ns = "lanes_" + map_name ;
00363           lane.id = (int32_t) i;
00364           lane.type = visualization_msgs::Marker::LINE_LIST;
00365           lane.action = visualization_msgs::Marker::ADD;
00366 
00367           // define lane boundary points: first left (0, 1), then right (2, 3)
00368           for (uint32_t j = 0;
00369                j < lane_data.polygons[i].poly.points.size(); ++j)
00370             {
00371               // convert Point32 message to Point (there should be a
00372               // better way)
00373               geometry_msgs::Point p;
00374               p.x = lane_data.polygons[i].poly.points[j].x;
00375               p.y = lane_data.polygons[i].poly.points[j].y;
00376               p.z = lane_data.polygons[i].poly.points[j].z;
00377               lane.points.push_back(p);
00378             }
00379 
00380           lane.scale.x = 0.1;               // 10cm lane boundaries
00381           lane.color = green;
00382           lane.lifetime = life;
00383 
00384           // Add these boundaries to the vector of markers to publish
00385           marks_msg_.markers.push_back(lane);
00386         }
00387 
00388       if (lane_data.polygons[i].contains_way)
00389         {
00390           visualization_msgs::Marker wp;
00391           wp.header.stamp = now;
00392           wp.header.frame_id = frame_id_;
00393 
00394           // publish way-points
00395           wp.ns = "waypoints_" + map_name;
00396           wp.id = (int32_t) i;
00397           wp.type = visualization_msgs::Marker::CYLINDER;
00398           wp.action = visualization_msgs::Marker::ADD;
00399 
00400           wp.pose.position = lane_data.polygons[i].midpoint;
00401           wp.pose.orientation = 
00402             tf::createQuaternionMsgFromYaw(lane_data.polygons[i].heading);
00403 
00404           wp.scale.x = 1.0;
00405           wp.scale.y = 1.0;
00406           wp.scale.z = 0.1;
00407           wp.lifetime = life;
00408 
00409           wp.color.a = 0.8;     // way-points are slightly transparent
00410           if (lane_data.polygons[i].is_stop)
00411             {
00412               // make stop way-points red
00413               wp.color.r = 1.0;
00414               wp.color.g = 0.0;
00415               wp.color.b = 0.0;
00416             }
00417           else
00418             {
00419               // make other way-points yellow
00420               wp.color.r = 1.0;
00421               wp.color.g = 1.0;
00422               wp.color.b = 0.0;
00423             }
00424 
00425           // Add this way-point to the vector of markers to publish
00426           marks_msg_.markers.push_back(wp);
00427         }
00428     }
00429 
00430   pub.publish(marks_msg_);
00431 }
00432 
00434 void MapLanesDriver::publishGlobalMap(void)
00435 {
00436   art_msgs::ArtLanes lane_data;
00437   if (0 == map_->getAllLanes(&lane_data))
00438     {
00439       ROS_WARN("no map data available to publish");
00440       return;
00441     }
00442 
00443   // the map is in the /map frame of reference with present time
00444   lane_data.header.stamp = ros::Time::now();
00445   lane_data.header.frame_id = frame_id_;
00446 
00447   ROS_INFO_STREAM("publishing " <<  lane_data.polygons.size()
00448                   <<" global roadmap polygons");
00449   roadmap_global_.publish(lane_data);
00450 #if 0 // only publish local map (for now)
00451   // publish global map with permanent duration
00452   publishMapMarks(mapmarks_, "global_roadmap", ros::Duration(), lane_data);
00453 #endif
00454 }
00455 
00457 void MapLanesDriver::publishLocalMap(void)
00458 {
00459   art_msgs::ArtLanes lane_data;
00460   if (0 != map_->getLanes(&lane_data, MapXY(odom_msg_.pose.pose.position)))
00461     {
00462       ROS_DEBUG("no map data available to publish");
00463       return;
00464     }
00465 
00466   // the map is in the /map frame of reference with time of the
00467   // latest odometry message
00468   lane_data.header.stamp = odom_msg_.header.stamp;
00469   lane_data.header.frame_id = frame_id_;
00470 
00471   ROS_DEBUG_STREAM("publishing " <<  lane_data.polygons.size()
00472                    <<" local roadmap polygons");
00473   roadmap_local_.publish(lane_data);
00474 
00475   // publish local map with temporary duration
00476   publishMapMarks(mapmarks_, "local_roadmap",
00477                   ros::Duration(art_msgs::ArtHertz::MAPLANES), lane_data);
00478 
00479   // publish local map with temporary duration
00480   publishMapCloud(roadmap_cloud_, lane_data);
00481 }
00482 
00484 void MapLanesDriver::Spin() 
00485 {
00486   publishGlobalMap();                   // publish global map once at start
00487 
00488   ros::Rate cycle(art_msgs::ArtHertz::MAPLANES); // set driver cycle rate
00489 
00490   // Loop publishing MapLanes state until driver Shutdown().
00491   while(ros::ok())
00492     {
00493       ros::spinOnce();                  // handle incoming messages
00494 
00495       if (initial_position_)
00496         {
00497           // Publish local roadmap
00498           publishLocalMap();
00499         }
00500 
00501       cycle.sleep();                    // sleep until next cycle
00502     }
00503 }
00504 
00509 bool MapLanesDriver::buildRoadMap(void)
00510 {
00511   if (rndf_name_ == "")
00512     {
00513       ROS_FATAL("required ~rndf parameter missing");
00514       return false;
00515     }
00516 
00517   RNDF *rndf = new RNDF(rndf_name_);
00518   
00519   if (!rndf->is_valid)
00520     {
00521       ROS_FATAL("RNDF not valid");
00522       delete rndf;
00523       return false;;
00524     }
00525 
00526   // Allocate a way-point graph.  Populate with nodes from the RNDF,
00527   // then fill in the MapXY coordinates relative to a UTM grid based
00528   // on the first way-point in the graph.
00529   graph_ = new Graph();
00530   rndf->populate_graph(*graph_);
00531   graph_->find_mapxy();
00532 
00533   // MapRNDF() saves a pointer to the Graph object, so we can't delete it here.
00534   // That is why graph_ is a class variable, deleted in the deconstructor.
00535   // TODO: fix this absurd interface
00536   int rc = map_->MapRNDF(graph_, poly_size_);
00537   delete rndf;
00538 
00539   if (rc != 0)
00540     {
00541       ROS_FATAL("cannot process RNDF! (%s)", strerror(rc));
00542       return false;
00543     }
00544 
00545   return true;
00546 }
00547 
00549 int main(int argc, char** argv)
00550 {
00551   ros::init(argc, argv, "maplanes");
00552   ros::NodeHandle node;
00553   MapLanesDriver dvr;
00554 
00555   if (dvr.Setup(node) != 0)
00556     return 2;
00557   if (!dvr.buildRoadMap())
00558     return 3;
00559   dvr.Spin();
00560   dvr.Shutdown();
00561 
00562   return 0;
00563 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:51