footstep_planning_vis.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2017, Alexander Stumpf, 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 Simulation, Systems Optimization and Robotics
00013 //       group, 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 #ifndef VIGIR_FOOTSTEP_PLANNING_VIS_H__
00030 #define VIGIR_FOOTSTEP_PLANNING_VIS_H__
00031 
00032 #include <ros/ros.h>
00033 #include <sensor_msgs/PointCloud2.h>
00034 #include <pcl_conversions/pcl_conversions.h>
00035 
00036 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00037 #include <vigir_footstep_planning_msgs/visualization.h>
00038 
00039 
00040 
00041 namespace vigir_footstep_planning
00042 {
00043 namespace vis
00044 {
00045 struct StepMsgVisCompare
00046 {
00047   bool operator() (const msgs::Step& lhs, const msgs::Step& rhs) const
00048   {
00049     double lhs_a[3];
00050     toArray(lhs, lhs_a);
00051     double rhs_a[3];
00052     toArray(rhs, rhs_a);
00053 
00054     // we can't visualize orientation, thus we ignore it
00055     for (size_t i = 0; i < 3; i++)
00056     {
00057       if (lhs_a[i] < rhs_a[i])
00058         return true;
00059       else if (lhs_a[i] > rhs_a[i])
00060         return false;
00061     }
00062     return false;
00063   }
00064 
00065   void toArray(const msgs::Step s, double (&a)[3]) const
00066   {
00067     a[0] = s.foot.pose.position.x;
00068     a[1] = s.foot.pose.position.y;
00069     a[2] = s.foot.pose.position.z;
00070   }
00071 };
00072 
00073 void publishFeet(ros::Publisher& pub, const msgs::Feet& feet, const geometry_msgs::Vector3& foot_size, const std_msgs::ColorRGBA& color);
00074 void publishStart(ros::Publisher& pub, const msgs::Feet& feet, const geometry_msgs::Vector3& foot_size);
00075 void publishGoal(ros::Publisher& pub, const msgs::Feet& feet, const geometry_msgs::Vector3& foot_size);
00076 
00077 void publishStepPlan(ros::Publisher& pub, const msgs::StepPlan& step_plan, const geometry_msgs::Vector3& foot_size, visualization_msgs::MarkerArray& last_step_plan_vis, bool add_step_index = true);
00078 void publishUpperBody(ros::Publisher& pub, const msgs::StepPlan& step_plan, const geometry_msgs::Vector3& upper_body_size, const geometry_msgs::Vector3& upper_body_origin_shift, visualization_msgs::MarkerArray& last_upper_body_vis, bool add_step_index = true);
00079 void publishPath(ros::Publisher& pub, const msgs::StepPlan& step_plan);
00080 
00081 void clearFeet(ros::Publisher& pub, const std_msgs::Header& header);
00082 void clearMarkerArray(ros::Publisher& pub, visualization_msgs::MarkerArray& last_step_plan_vis);
00083 void clearPath(ros::Publisher& pub, const std_msgs::Header& header);
00084 
00085 void publishRecentlyVistedSteps(ros::Publisher& pub, const std::vector<msgs::Step>& recently_visited_steps, const std_msgs::Header& header);
00086 void publishVistedSteps(ros::Publisher& pub, const std::vector<msgs::Step>& visited_steps, const std_msgs::Header& header);
00087 void publishVistedSteps(ros::Publisher& pub, const std::set<msgs::Step, StepMsgVisCompare>& visited_steps, const std_msgs::Header& header);
00088 }
00089 }
00090 
00091 #endif


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56