footstep_planning_vis.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_lib/visualization/footstep_planning_vis.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 namespace vis
00006 {
00007 void publishFeet(ros::Publisher& pub, const msgs::Feet& feet, const geometry_msgs::Vector3& foot_size, const std_msgs::ColorRGBA& color)
00008 {
00009   visualization_msgs::MarkerArray marker_array;
00010   msgs::feetToFootMarkerArray(feet, foot_size, color, marker_array);
00011   pub.publish(marker_array);
00012 }
00013 
00014 void publishStart(ros::Publisher& pub, const msgs::Feet& feet, const geometry_msgs::Vector3& foot_size)
00015 {
00016   std_msgs::ColorRGBA color;
00017   color.r = 0.0;
00018   color.g = 0.7;
00019   color.b = 0.0;
00020   color.a = 0.6;
00021   publishFeet(pub, feet, foot_size, color);
00022 }
00023 
00024 void publishGoal(ros::Publisher& pub, const msgs::Feet& feet, const geometry_msgs::Vector3& foot_size)
00025 {
00026   std_msgs::ColorRGBA color;
00027   color.r = 0.7;
00028   color.g = 0.0;
00029   color.b = 0.0;
00030   color.a = 0.6;
00031   publishFeet(pub, feet, foot_size, color);
00032 }
00033 
00034 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)
00035 {
00036   visualization_msgs::MarkerArray step_plan_vis;
00037   msgs::stepPlanToFootMarkerArray(step_plan, foot_size, step_plan_vis, add_step_index);
00038 
00039   // resize marker array
00040   if (last_step_plan_vis.markers.size() < step_plan_vis.markers.size())
00041     last_step_plan_vis.markers.resize(step_plan_vis.markers.size());
00042 
00043   // overwrite old markers
00044   for (size_t i = 0; i < step_plan_vis.markers.size(); i++)
00045     last_step_plan_vis.markers[i] = step_plan_vis.markers[i];
00046 
00047   // set needless markers to be deleted
00048   for (size_t i = step_plan_vis.markers.size(); i < last_step_plan_vis.markers.size(); i++)
00049     last_step_plan_vis.markers[i].action = visualization_msgs::Marker::DELETE;
00050 
00051   // finally publish new markers
00052   pub.publish(last_step_plan_vis);
00053 
00054   // delete old markers from array
00055   last_step_plan_vis.markers.resize(step_plan_vis.markers.size());
00056 }
00057 
00058 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)
00059 {
00060   visualization_msgs::MarkerArray upper_body_vis;
00061   msgs::stepPlanToUpperBodyMarkerArray(step_plan, upper_body_size, upper_body_origin_shift, upper_body_vis, add_step_index);
00062 
00063   // resize marker array
00064   if (last_upper_body_vis.markers.size() < upper_body_vis.markers.size())
00065     last_upper_body_vis.markers.resize(upper_body_vis.markers.size());
00066 
00067   // overwrite old markers
00068   for (size_t i = 0; i < upper_body_vis.markers.size(); i++)
00069     last_upper_body_vis.markers[i] = upper_body_vis.markers[i];
00070 
00071   // set needless markers to be deleted
00072   for (size_t i = upper_body_vis.markers.size(); i < last_upper_body_vis.markers.size(); i++)
00073     last_upper_body_vis.markers[i].action = visualization_msgs::Marker::DELETE;
00074 
00075   // finally publish new markers
00076   pub.publish(last_upper_body_vis);
00077 
00078   // delete old markers from array
00079   last_upper_body_vis.markers.resize(upper_body_vis.markers.size());
00080 }
00081 
00082 void publishPath(ros::Publisher& pub, const msgs::StepPlan& step_plan)
00083 {
00084   nav_msgs::Path path;
00085   msgs::stepPlanToPath(step_plan, path);
00086   pub.publish(path);
00087 }
00088 
00089 void clearFeet(ros::Publisher& pub, const std_msgs::Header& header)
00090 {
00091   visualization_msgs::MarkerArray marker_array;
00092   visualization_msgs::Marker marker;
00093 
00094   marker.header = header;
00095   marker.type = visualization_msgs::Marker::CUBE;
00096   marker.action = visualization_msgs::Marker::DELETE;
00097 
00098   marker.id = 0;
00099   marker_array.markers.push_back(marker);
00100   marker.id++;
00101   marker_array.markers.push_back(marker);
00102 
00103   pub.publish(marker_array);
00104 }
00105 
00106 void clearMarkerArray(ros::Publisher& pub, visualization_msgs::MarkerArray& last_step_plan_vis)
00107 {
00108   for (std::vector<visualization_msgs::Marker>::iterator itr = last_step_plan_vis.markers.begin(); itr != last_step_plan_vis.markers.end(); itr++)
00109     itr->action = visualization_msgs::Marker::DELETE;
00110   pub.publish(last_step_plan_vis);
00111 }
00112 
00113 void clearPath(ros::Publisher& pub, const std_msgs::Header& header)
00114 {
00115   nav_msgs::Path path;
00116   path.header = header;
00117   pub.publish(path);
00118 }
00119 
00120 void publishRecentlyVistedSteps(ros::Publisher& pub, const std::vector<msgs::Step>& recently_visited_steps, const std_msgs::Header& header)
00121 {
00122   // populate point cloud
00123   pcl::PointCloud<pcl::PointXYZ> point_cloud;
00124   point_cloud.resize(recently_visited_steps.size());
00125 
00126   for (unsigned long i = 0; i < recently_visited_steps.size(); i++)
00127   {
00128     pcl::PointXYZ& point = point_cloud.points[i];
00129     copyPosition(recently_visited_steps[i].foot.pose.position, point);
00130   }
00131 
00132   // publish as point cloud msg
00133   sensor_msgs::PointCloud2 point_cloud_msgs;
00134   pcl::toROSMsg(point_cloud, point_cloud_msgs);
00135   point_cloud_msgs.header = header;
00136   pub.publish(point_cloud_msgs);
00137 }
00138 
00139 void publishVistedSteps(ros::Publisher& pub, const std::vector<msgs::Step>& visited_steps, const std_msgs::Header& header)
00140 {
00141   // populate point cloud
00142   pcl::PointCloud<pcl::PointXYZI> point_cloud;
00143   point_cloud.resize(visited_steps.size());
00144 
00145   ros::Time current_time = ros::Time::now();
00146   unsigned long i = 0;
00147   for (std::vector<msgs::Step>::const_iterator itr = visited_steps.begin(); itr != visited_steps.end(); itr++, i++)
00148   {
00149     pcl::PointXYZI& point = point_cloud.points[i];
00150     copyPosition(itr->foot.pose.position, point);
00151     point.intensity = std::min(5.0, (current_time - itr->header.stamp).toSec());
00152   }
00153 
00154   // publish as point cloud msg
00155   sensor_msgs::PointCloud2 point_cloud_msgs;
00156   pcl::toROSMsg(point_cloud, point_cloud_msgs);
00157   point_cloud_msgs.header = header;
00158   pub.publish(point_cloud_msgs);
00159 }
00160 
00161 void publishVistedSteps(ros::Publisher& pub, const std::set<msgs::Step, StepMsgVisCompare>& visited_steps, const std_msgs::Header& header)
00162 {
00163   std::vector<msgs::Step> vec;
00164   for (std::set<msgs::Step, StepMsgVisCompare>::const_iterator itr = visited_steps.begin(); itr != visited_steps.end(); itr++)
00165     vec.push_back(*itr);
00166   publishVistedSteps(pub, vec, header);
00167 }
00168 }
00169 }


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