ros_conversion.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00040 #include <topological_roadmap/ros_conversion.h>
00041 #include <graph_mapping_utils/to_string.h>
00042 #include <visualization_msgs/Marker.h>
00043 #include <tf/exceptions.h>
00044 #include <boost/foreach.hpp>
00045 
00046 namespace topological_roadmap
00047 {
00048 namespace vm=visualization_msgs;
00049 namespace gm=geometry_msgs;
00050 namespace msg=topological_nav_msgs;
00051 namespace gmu=graph_mapping_utils;
00052 
00053 typedef std::vector<unsigned> Path;
00054 
00055 msg::TopologicalRoadmap::Ptr toRosMessage (const Roadmap& r)
00056 {
00057   msg::TopologicalRoadmap::Ptr m =
00058     boost::make_shared<msg::TopologicalRoadmap>();
00059   BOOST_FOREACH (const GraphVertex v, vertices(r)) 
00060     m->nodes.push_back(r[v]);
00061   BOOST_FOREACH (const GraphEdge e, edges(r)) 
00062     m->edges.push_back(r[e]);
00063 
00064   return m;
00065 }
00066 
00067 
00068 Roadmap fromRosMessage (const msg::TopologicalRoadmap& m)
00069 {
00070   Roadmap r;
00071 
00072   BOOST_FOREACH (const msg::RoadmapNode& info, m.nodes) 
00073     r.addNode(info);
00074   BOOST_FOREACH (const msg::RoadmapEdge& info, m.edges) 
00075     r.addEdge(info);
00076   r.recomputeIndexes();
00077 
00078   return r;
00079 }
00080 
00081 
00082 gm::Point nodePos (const Roadmap& r, tf::TransformListener& tf,
00083                    const std::string& frame, const unsigned n)
00084 {
00085   gm::PointStamped in, out;
00086   const msg::RoadmapNode& info = r.nodeInfo(n);
00087   in.point = info.position;
00088   in.header.stamp = ros::Time();
00089   in.header.frame_id = topological_map_2d::gridFrame(info.grid);
00090   tf.transformPoint(frame, in, out);
00091   return out.point;
00092 }
00093 
00094 void visualizeNodes (const Roadmap& r, ros::Publisher& pub,
00095                      tf::TransformListener& tf, const std::string& frame,
00096                      const bool use_node_ids)
00097 {
00098   vm::Marker nodes;
00099   nodes.header.frame_id = frame;
00100   nodes.header.stamp = ros::Time::now();
00101   nodes.id = 1;
00102   nodes.ns = "roadmap";
00103   nodes.action = vm::Marker::ADD;
00104   nodes.scale.x = nodes.scale.y = nodes.scale.z = 0.1;
00105   nodes.color.r = 1.0;
00106   nodes.color.g = 1.0;
00107   nodes.color.b = 0.0;
00108   nodes.color.a = 1.0;
00109   nodes.pose.orientation.w = 1.0;
00110 
00111   if (use_node_ids)
00112     nodes.type = vm::Marker::SPHERE;
00113   else
00114     nodes.type = vm::Marker::SPHERE_LIST;
00115   
00116   BOOST_FOREACH (const GraphVertex& v, vertices(r)) 
00117   {
00118     try
00119     {
00120       if (use_node_ids)
00121       {
00122         nodes.id = r[v].id;
00123         nodes.pose.position = nodePos(r, tf, frame, r[v].id);
00124         pub.publish(nodes);
00125       }
00126       else
00127       {
00128         nodes.points.push_back(nodePos(r, tf, frame, r[v].id));
00129       }
00130     }
00131     catch (tf::TransformException& e)
00132     {
00133       ROS_WARN_STREAM_THROTTLE (1.0, "TF exception " << e.what() <<
00134                                 " when visualizing roadmap");
00135     }
00136   }
00137 
00138   if (!use_node_ids)
00139     pub.publish(nodes);
00140 }
00141 
00142 void visualizeEdges (const Roadmap& r, ros::Publisher& pub,
00143                      tf::TransformListener& tf, const std::string& frame,
00144                      const Path& plan)
00145 {
00146   vm::Marker m;
00147   m.header.frame_id = frame;
00148   m.header.stamp = ros::Time::now();
00149   m.id = 0;
00150   m.ns = "roadmap";
00151   m.type = vm::Marker::LINE_LIST;
00152   m.action = vm::Marker::ADD;
00153   m.scale.x = 0.05;
00154   m.color.r = 0.15;
00155   m.color.g = 0.55;
00156   m.color.b = 0.15;
00157   m.color.a = 1.0;
00158 
00159   vm::Marker path = m;
00160   path.id = 99999; // Avoid conflicting with node ids
00161   path.color.r = 0.75;
00162   path.color.g = 0.2;
00163   path.scale.x = 0.07;
00164 
00165   BOOST_FOREACH (const GraphEdge& e, boost::edges(r)) 
00166   {
00167     try
00168     {
00169       const unsigned src_node = r[e].src;
00170       const unsigned dest_node = r[e].dest;
00171       const gm::Point src = nodePos(r, tf, frame, src_node);
00172       const gm::Point dest = nodePos(r, tf, frame, dest_node);
00173       bool on_path = false;
00174       for (unsigned i=0; i+1<plan.size(); i++)
00175       {
00176         if ((plan[i] == r[e].src && plan[i+1] == r[e].dest) ||
00177             (plan[i+1] == r[e].src && plan[i] == r[e].dest))
00178           on_path=true;
00179       }
00180       if (on_path) 
00181       {
00182         path.points.push_back(src);
00183         path.points.push_back(dest);
00184       }
00185       else
00186       {
00187         m.points.push_back(src);
00188         m.points.push_back(dest);
00189       }
00190     }
00191     catch (tf::TransformException& e)
00192     {
00193       ROS_WARN_STREAM_THROTTLE (1.0, "TF exception " << e.what() <<
00194                                 " when visualizing roadmap");
00195     }
00196   }
00197 
00198   pub.publish(m);
00199   pub.publish(path);
00200 }
00201 
00202 
00203 
00204 void visualize (const Roadmap& r, ros::Publisher& pub,
00205                 tf::TransformListener& tf, const std::string& frame,
00206                 const bool use_node_ids, const Path& path)
00207 {
00208   visualizeNodes(r, pub, tf, frame, use_node_ids);
00209   visualizeEdges(r, pub, tf, frame, path);
00210 }
00211 
00212 
00213 } // namespace


topological_roadmap
Author(s): Bhaskara Marthi
autogenerated on Sun Jan 5 2014 11:39:33