trajectory_geotiff_plugin.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Gregor Gebhardt, TU Darmstadt
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 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_geotiff/map_writer_interface.h>
00030 #include <hector_geotiff/map_writer_plugin_interface.h>
00031 
00032 #include <ros/ros.h>
00033 #include <hector_nav_msgs/GetRobotTrajectory.h>
00034 
00035 #include <pluginlib/class_loader.h>
00036 #include <fstream>
00037 
00038 namespace hector_geotiff_plugins
00039 {
00040 
00041 using namespace hector_geotiff;
00042 
00043 class TrajectoryMapWriter : public MapWriterPluginInterface
00044 {
00045 public:
00046   TrajectoryMapWriter();
00047   virtual ~TrajectoryMapWriter();
00048 
00049   virtual void initialize(const std::string& name);
00050   virtual void draw(MapWriterInterface *interface);
00051 
00052 protected:
00053   ros::NodeHandle nh_;
00054   ros::ServiceClient service_client_;
00055 
00056   bool initialized_;
00057   std::string name_;
00058   bool draw_all_objects_;
00059   std::string class_id_;
00060   int path_color_r_;
00061   int path_color_g_;
00062   int path_color_b_;
00063 };
00064 
00065 TrajectoryMapWriter::TrajectoryMapWriter()
00066     : initialized_(false)
00067 {}
00068 
00069 TrajectoryMapWriter::~TrajectoryMapWriter()
00070 {}
00071 
00072 void TrajectoryMapWriter::initialize(const std::string& name)
00073 {
00074   ros::NodeHandle plugin_nh("~/" + name);
00075   std::string service_name_;
00076 
00077 
00078   plugin_nh.param("service_name", service_name_, std::string("trajectory"));
00079   plugin_nh.param("path_color_r", path_color_r_, 120);
00080   plugin_nh.param("path_color_g", path_color_g_, 0);
00081   plugin_nh.param("path_color_b", path_color_b_, 240);
00082 
00083   service_client_ = nh_.serviceClient<hector_nav_msgs::GetRobotTrajectory>(service_name_);
00084 
00085   initialized_ = true;
00086   this->name_ = name;
00087   ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str());
00088 }
00089 
00090 void TrajectoryMapWriter::draw(MapWriterInterface *interface)
00091 {
00092     if(!initialized_) return;
00093 
00094     hector_nav_msgs::GetRobotTrajectory srv_path;
00095     if (!service_client_.call(srv_path)) {
00096       ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str());
00097       return;
00098     }
00099 
00100     std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
00101 
00102     size_t size = traj_vector.size();
00103 
00104     std::vector<Eigen::Vector2f> pointVec;
00105     pointVec.resize(size);
00106 
00107     for (size_t i = 0; i < size; ++i){
00108       const geometry_msgs::PoseStamped& pose (traj_vector[i]);
00109 
00110       pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
00111     }
00112 
00113     if (size > 0){
00114       //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
00115       Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
00116       interface->drawPath(startVec, pointVec, path_color_r_, path_color_g_, path_color_b_);
00117     }
00118 }
00119 
00120 } // namespace
00121 
00122 //register this planner as a MapWriterPluginInterface plugin
00123 #include <pluginlib/class_list_macros.h>
00124 #ifdef PLUGINLIB_EXPORT_CLASS
00125   PLUGINLIB_EXPORT_CLASS(hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface)
00126 #else
00127   PLUGINLIB_DECLARE_CLASS(hector_geotiff_plugins, TrajectoryMapWriter, hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface)
00128 #endif


hector_geotiff_plugins
Author(s): Stefan Kohlbrecher , Gregor Gebhardt
autogenerated on Thu Jun 6 2019 20:12:36