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 };
00061 
00062 TrajectoryMapWriter::TrajectoryMapWriter()
00063     : initialized_(false)
00064 {}
00065 
00066 TrajectoryMapWriter::~TrajectoryMapWriter()
00067 {}
00068 
00069 void TrajectoryMapWriter::initialize(const std::string& name)
00070 {
00071   ros::NodeHandle plugin_nh("~/" + name);
00072   std::string service_name_;
00073 
00074   plugin_nh.param("service_name", service_name_, std::string("trajectory"));
00075 
00076   service_client_ = nh_.serviceClient<hector_nav_msgs::GetRobotTrajectory>(service_name_);
00077 
00078   initialized_ = true;
00079   this->name_ = name;
00080   ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str());
00081 }
00082 
00083 void TrajectoryMapWriter::draw(MapWriterInterface *interface)
00084 {
00085     if(!initialized_) return;
00086 
00087     hector_nav_msgs::GetRobotTrajectory srv_path;
00088     if (!service_client_.call(srv_path)) {
00089       ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str());
00090       return;
00091     }
00092 
00093     std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
00094 
00095     size_t size = traj_vector.size();
00096 
00097     std::vector<Eigen::Vector2f> pointVec;
00098     pointVec.resize(size);
00099 
00100     for (size_t i = 0; i < size; ++i){
00101       const geometry_msgs::PoseStamped& pose (traj_vector[i]);
00102 
00103       pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
00104     }
00105 
00106     if (size > 0){
00107       //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
00108       Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
00109       interface->drawPath(startVec, pointVec);
00110     }
00111 }
00112 
00113 } // namespace
00114 
00115 //register this planner as a MapWriterPluginInterface plugin
00116 #include <pluginlib/class_list_macros.h>
00117 #ifdef PLUGINLIB_EXPORT_CLASS
00118   PLUGINLIB_EXPORT_CLASS(hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface)
00119 #else
00120   PLUGINLIB_DECLARE_CLASS(hector_geotiff_plugins, TrajectoryMapWriter, hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface)
00121 #endif


hector_geotiff_plugins
Author(s): Gregor Gebhardt
autogenerated on Mon Oct 6 2014 00:35:02