footstep_planning_vis_node.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_lib/visualization/footstep_planning_vis_node.h>
00002 
00003 #include <vigir_footstep_planning_lib/helper.h>
00004 
00005 
00006 
00007 namespace vigir_footstep_planning
00008 {
00009 FootstepPlanningVisNode::FootstepPlanningVisNode()
00010 {
00011   ros::NodeHandle nh;
00012 
00013   // load param
00014   getFootSize(nh, foot_size);
00015   getUpperBodySize(nh, upper_body_size);
00016   getUpperBodyOriginShift(nh, upper_body_origin_shift);
00017 
00018   // subscribe topics
00019   step_plan_request_vis_sub = nh.subscribe<msgs::StepPlanRequest>("vis/step_plan_request", 1, &FootstepPlanningVisNode::stepPlanRequestVisCallback, this);
00020   step_plan_vis_sub = nh.subscribe<msgs::StepPlan>("vis/step_plan", 1, &FootstepPlanningVisNode::stepPlanVisCallback, this);
00021   planning_feedback_sub = nh.subscribe<msgs::PlanningFeedback>("planning_feedback", 1, &FootstepPlanningVisNode::planningFeedbackCallback, this);
00022 
00023   // publish topics
00024   step_plan_vis_pub = nh.advertise<visualization_msgs::MarkerArray>("vis/step_plan_vis", 1, true);
00025   upper_body_vis_pub = nh.advertise<visualization_msgs::MarkerArray>("vis/upper_body_vis", 1, true);
00026   step_plan_path_pub = nh.advertise<nav_msgs::Path>("vis/step_plan_path", 1, true);
00027   start_feet_pose_pub = nh.advertise<visualization_msgs::MarkerArray>("vis/start_feet_pose", 1, true);
00028   goal_feet_pose_pub = nh.advertise<visualization_msgs::MarkerArray>("vis/goal_feet_pose", 1, true);
00029   visited_steps_pub = nh.advertise<sensor_msgs::PointCloud2>("feedback/visited_steps", 1);
00030   total_visited_steps_pub = nh.advertise<sensor_msgs::PointCloud2>("feedback/total_visited_steps", 1);
00031   current_step_plan_pub = nh.advertise<visualization_msgs::MarkerArray>("feedback/current_step_plan", 1);
00032 
00033   // start service clients
00034   transform_foot_pose_client = nh.serviceClient<msgs::TransformFootPoseService>("transform_foot_pose");
00035   transform_feet_poses_client = nh.serviceClient<msgs::TransformFeetPosesService>("transform_feet_poses");
00036   transform_step_plan_client = nh.serviceClient<msgs::TransformStepPlanService>("transform_step_plan");
00037 }
00038 
00039 FootstepPlanningVisNode::~FootstepPlanningVisNode()
00040 {
00041 }
00042 
00043 void FootstepPlanningVisNode::stepPlanRequestVisCallback(const msgs::StepPlanRequestConstPtr& step_plan_request)
00044 {
00045   // clear old vis
00046   clearVisualization(step_plan_request->header);
00047 
00048   msgs::Feet start = step_plan_request->start;
00049   msgs::Feet goal = step_plan_request->goal;
00050 
00051   // transform start pose to planner frame and visualize
00052   if (!start.header.frame_id.empty())
00053   {
00054     transformToPlannerFrame(start, transform_feet_poses_client);
00055     vis::publishStart(start_feet_pose_pub, start, foot_size);
00056   }
00057 
00058   // transform goal pose to planner frame and visualize
00059   if (!goal.header.frame_id.empty())
00060   {
00061     transformToPlannerFrame(goal, transform_feet_poses_client);
00062     vis::publishGoal(goal_feet_pose_pub, goal, foot_size);
00063   }
00064 }
00065 
00066 void FootstepPlanningVisNode::stepPlanVisCallback(const msgs::StepPlanConstPtr& step_plan)
00067 {
00068   // transform to planner frame
00069   msgs::StepPlan step_plan_t = *step_plan;
00070   transformToPlannerFrame(step_plan_t, transform_step_plan_client);
00071 
00072   // visualize plan
00073   vis::publishStepPlan(step_plan_vis_pub, step_plan_t, foot_size, last_step_plan_vis);
00074   vis::publishUpperBody(upper_body_vis_pub, step_plan_t, upper_body_size, upper_body_origin_shift, last_upper_body_vis);
00075   vis::publishPath(step_plan_path_pub, step_plan_t);
00076 }
00077 
00078 void FootstepPlanningVisNode::planningFeedbackCallback(const msgs::PlanningFeedbackConstPtr& planning_feedback)
00079 {
00080   // collect data
00081   for (std::vector<msgs::Step>::const_iterator itr = planning_feedback->visited_steps.begin(); itr != planning_feedback->visited_steps.end(); itr++)
00082   {
00083     msgs::Step step = *itr;
00084     total_visited_steps.erase(step);
00085     total_visited_steps.insert(step);
00086   }
00087 
00088   if (visited_steps_pub.getNumSubscribers() > 0)
00089     vis::publishRecentlyVistedSteps(visited_steps_pub, planning_feedback->visited_steps, planning_feedback->header);
00090 
00091   if (total_visited_steps_pub.getNumSubscribers() > 0)
00092     vis::publishVistedSteps(total_visited_steps_pub, total_visited_steps, planning_feedback->header);
00093 
00094   msgs::StepPlan current_step_plan = planning_feedback->current_step_plan;
00095   transformToPlannerFrame(current_step_plan, transform_step_plan_client);
00096   vis::publishStepPlan(current_step_plan_pub, current_step_plan, foot_size, last_current_step_plan_vis, false);
00097 }
00098 
00099 void FootstepPlanningVisNode::clearVisualization(const std_msgs::Header& header)
00100 {
00101   total_visited_steps.clear();
00102 
00103   // last start and goal pose from request
00104   vis::clearFeet(start_feet_pose_pub, header);
00105   vis::clearFeet(goal_feet_pose_pub, header);
00106 
00107   // last planner feedback
00108   vis::clearMarkerArray(current_step_plan_pub, last_current_step_plan_vis);
00109   sensor_msgs::PointCloud2 point_cloud_msgs;
00110   point_cloud_msgs.header = header;
00111   visited_steps_pub.publish(point_cloud_msgs);
00112   total_visited_steps_pub.publish(point_cloud_msgs);
00113 
00114   // last solution
00115   vis::clearMarkerArray(step_plan_vis_pub, last_step_plan_vis);
00116   vis::clearMarkerArray(upper_body_vis_pub, last_upper_body_vis);
00117   vis::clearPath(step_plan_path_pub, header);
00118 }
00119 }
00120 
00121 int main(int argc, char **argv)
00122 {
00123   ros::init(argc, argv, "footstep_planning_vis_node");
00124   vigir_footstep_planning::FootstepPlanningVisNode vis_node;
00125   ros::spin();
00126 
00127   return 0;
00128 }


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