visualization.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_msgs/visualization.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 namespace msgs
00006 {
00007 void footToFootMarker(const Foot& foot, const geometry_msgs::Vector3& foot_size, const std_msgs::ColorRGBA& color, visualization_msgs::Marker& marker)
00008 {
00009   marker.header = foot.header;
00010   marker.ns = "vigir_footstep_planning/step_plan";
00011   marker.type = visualization_msgs::Marker::CUBE;
00012   marker.action = visualization_msgs::Marker::ADD;
00013 
00014   marker.pose = foot.pose;
00015   marker.pose.position.z += 0.5*foot_size.z;
00016   marker.scale = foot_size;
00017   marker.color = color;
00018 
00019   marker.lifetime = ros::Duration();
00020 }
00021 
00022 void feetToFootMarkerArray(const Feet& feet, const geometry_msgs::Vector3& foot_size, const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& marker_array)
00023 {
00024   visualization_msgs::Marker marker;
00025   marker_array.markers.clear();
00026 
00027   footToFootMarker(feet.left, foot_size, color, marker);
00028   marker_array.markers.push_back(marker);
00029   marker.id++;
00030 
00031   footToFootMarker(feet.right, foot_size, color, marker);
00032   marker_array.markers.push_back(marker);
00033 }
00034 
00035 void stepToFootMarker(const Step& step, const geometry_msgs::Vector3& foot_size, const std_msgs::ColorRGBA& color, visualization_msgs::Marker& marker)
00036 {
00037   footToFootMarker(step.foot, foot_size, color, marker);
00038 }
00039 
00040 void stepPlanToFootMarkerArray(const std::vector<Step>& steps, const geometry_msgs::Vector3& foot_size, visualization_msgs::MarkerArray& marker_array, bool add_step_index)
00041 {
00042   std_msgs::ColorRGBA color;
00043   color.r = 0.0;
00044   color.g = 0.0;
00045   color.b = 0.0;
00046   color.a = 0.6;
00047 
00048   visualization_msgs::Marker marker;
00049 
00050   marker_array.markers.clear();
00051 
00052   for (std::vector<Step>::const_iterator itr = steps.begin(); itr != steps.end(); itr++)
00053   {
00054     // colorize
00055     color.r = itr->foot.foot_index == Foot::LEFT ? 1.0 : 0.0;
00056     color.g = itr->foot.foot_index == Foot::LEFT ? 0.0 : 1.0;
00057 
00058     // transform
00059     stepToFootMarker(*itr, foot_size, color, marker);
00060     marker_array.markers.push_back(marker);
00061     marker.id++;
00062 
00063     // add text
00064     if (add_step_index)
00065     {
00066       marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00067       marker.action = visualization_msgs::Marker::ADD;
00068       marker.text = boost::lexical_cast<std::string>(itr->step_index);
00069       marker.scale.z *= 3;
00070       marker.color.r = 1.0;
00071       marker.color.g = 1.0;
00072       marker.color.b = 1.0;
00073       marker.color.a = 0.7;
00074 
00075       marker_array.markers.push_back(marker);
00076       marker.id++;
00077     }
00078   }
00079 }
00080 
00081 void stepPlanToFootMarkerArray(const StepPlan& step_plan, const geometry_msgs::Vector3& foot_size, visualization_msgs::MarkerArray& marker_array, bool add_step_index)
00082 {
00083   stepPlanToFootMarkerArray(step_plan.steps, foot_size, marker_array, add_step_index);
00084 }
00085 
00086 void feetToUpperBodyMarker(const Feet& feet, const geometry_msgs::Vector3& upper_body_size, const geometry_msgs::Vector3& upper_body_origin_shift, const std_msgs::ColorRGBA& color, visualization_msgs::Marker& marker, bool flat)
00087 {
00088   marker.header = feet.header;
00089   marker.ns = "vigir_footstep_planning/upper_body";
00090   marker.type = visualization_msgs::Marker::CUBE;
00091   marker.action = visualization_msgs::Marker::ADD;
00092 
00093   // approximate upper body position
00094   marker.pose.position.x = 0.5 * (feet.left.pose.position.x + feet.right.pose.position.x);
00095   marker.pose.position.y = 0.5 * (feet.left.pose.position.y + feet.right.pose.position.y);
00096   marker.pose.position.z = 0.5 * (feet.left.pose.position.z + feet.right.pose.position.z);
00097   marker.pose.orientation = tf::createQuaternionMsgFromYaw(0.5 * (tf::getYaw(feet.left.pose.orientation) + tf::getYaw(feet.right.pose.orientation)));
00098 
00099   // determine shift of polygon based on orientation
00100   tf::Transform transform;
00101   tf::poseMsgToTF(marker.pose, transform);
00102   tf::Vector3 shifted;
00103   tf::vector3MsgToTF(upper_body_origin_shift, shifted);
00104   shifted = transform * shifted;
00105 
00106   marker.pose.position.x = shifted.getX();
00107   marker.pose.position.y = shifted.getY();
00108   marker.pose.position.z = shifted.getZ();
00109 
00110   // finalize marker
00111   marker.pose.position.z += flat ? 0.01 : 0.5*upper_body_size.z;
00112   marker.scale = upper_body_size;
00113   if (flat)
00114     marker.scale.z = 0.02;
00115   marker.color = color;
00116 
00117   marker.lifetime = ros::Duration();
00118 }
00119 
00120 void stepPlanToUpperBodyMarkerArray(const std::vector<Step>& steps, const geometry_msgs::Vector3& upper_body_size, const geometry_msgs::Vector3& upper_body_origin_shift, visualization_msgs::MarkerArray& marker_array, bool add_step_index)
00121 {
00122   std_msgs::ColorRGBA color;
00123   color.r = 0.0;
00124   color.g = 0.0;
00125   color.b = 0.4;
00126   color.a = 0.2;
00127 
00128   visualization_msgs::Marker marker;
00129 
00130   marker_array.markers.clear();
00131 
00132   Feet feet;
00133   for (std::vector<Step>::const_iterator itr = steps.begin(); itr != steps.end(); itr++)
00134   {
00135     const Step& step = *itr;
00136     if (step.foot.foot_index == Foot::LEFT)
00137       feet.left = step.foot;
00138     else
00139       feet.right = step.foot;
00140 
00141     if (itr == steps.begin())
00142     {
00143       feet.header = step.header;
00144       continue;
00145     }
00146 
00147     // transform
00148     feetToUpperBodyMarker(feet, upper_body_size, upper_body_origin_shift, color, marker);
00149     marker_array.markers.push_back(marker);
00150     marker.id++;
00151 
00152     // add text
00153     if (add_step_index)
00154     {
00155       marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00156       marker.action = visualization_msgs::Marker::ADD;
00157       marker.text = boost::lexical_cast<std::string>(itr->step_index);
00158       marker.scale.z *= 0.1;
00159       marker.color.r = 1.0;
00160       marker.color.g = 1.0;
00161       marker.color.b = 1.0;
00162       marker.color.a = 0.7;
00163 
00164       marker_array.markers.push_back(marker);
00165       marker.id++;
00166     }
00167   }
00168 }
00169 
00170 void stepPlanToUpperBodyMarkerArray(const StepPlan& step_plan, const geometry_msgs::Vector3& upper_body_size, const geometry_msgs::Vector3& upper_body_origin_shift, visualization_msgs::MarkerArray& marker_array, bool add_step_index)
00171 {
00172   stepPlanToUpperBodyMarkerArray(step_plan.steps, upper_body_size, upper_body_origin_shift, marker_array, add_step_index);
00173 }
00174 
00175 void stepPlanToPath(const StepPlan& step_plan, nav_msgs::Path& path)
00176 {
00177   path.poses.clear();
00178 
00179   path.header = step_plan.header;
00180   for (std::vector<Step>::const_iterator itr = step_plan.steps.begin(); itr != step_plan.steps.end(); itr++)
00181   {
00182     geometry_msgs::PoseStamped pose;
00183     pose.header = step_plan.header;
00184     pose.pose = itr->foot.pose;
00185     path.poses.push_back(pose);
00186   }
00187 }
00188 }
00189 }


vigir_footstep_planning_msgs
Author(s): Alexander Stumpf
autogenerated on Sat Jul 8 2017 02:21:30