vigir_global_footstep_planner: global_footstep_planner

Go to the documentation of this file.
00001 #include <vigir_global_footstep_planner/global_footstep_planner_node.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 GlobalFootstepPlannerNode::GlobalFootstepPlannerNode(ros::NodeHandle& nh)
00006   : FootstepPlannerNode(nh)
00007 {
00008 }
00009 
00010 GlobalFootstepPlannerNode::~GlobalFootstepPlannerNode()
00011 {
00012 }
00013 
00014 void GlobalFootstepPlannerNode::init(ros::NodeHandle& nh)
00015 {
00016   // init basics
00017   FootstepPlannerNode::init(nh);
00018 
00019   // init global footstep planner
00020   global_footstep_planner.reset(new GlobalFootstepPlanner(footstep_planner));
00021 
00022   // subscribe topics
00023   edit_step_sub = nh.subscribe("edit_step", 1, &GlobalFootstepPlannerNode::editStep, this);
00024   set_step_plan_sub = nh.subscribe("set_step_plan", 1, &GlobalFootstepPlannerNode::setStepPlan, this);
00025   stitch_step_plan_sub = nh.subscribe("stitch_step_plan", 1, &GlobalFootstepPlannerNode::stitchStepPlan, this);
00026 
00027   // start own services
00028   edit_step_srv = nh.advertiseService("edit_step", &GlobalFootstepPlannerNode::editStepService, this);
00029   set_step_plan_srv = nh.advertiseService("set_step_plan", &GlobalFootstepPlannerNode::setStepPlanService, this);
00030   get_step_plan_srv = nh.advertiseService("get_step_plan", &GlobalFootstepPlannerNode::getStepPlanService, this);
00031   stitch_step_plan_srv = nh.advertiseService("stitch_step_plan", &GlobalFootstepPlannerNode::stitchStepPlanService, this);
00032 
00033   // init action servers
00034   edit_step_as = SimpleActionServer<msgs::EditStepAction>::create(nh, "edit_step", true, boost::bind(&GlobalFootstepPlannerNode::editStepAction, this, boost::ref(edit_step_as)));
00035   set_step_plan_as = SimpleActionServer<msgs::SetStepPlanAction>::create(nh, "set_step_plan", true, boost::bind(&GlobalFootstepPlannerNode::setStepPlanAction, this, boost::ref(set_step_plan_as)));
00036   get_step_plan_as = SimpleActionServer<msgs::GetStepPlanAction>::create(nh, "get_step_plan", true, boost::bind(&GlobalFootstepPlannerNode::getStepPlanAction, this, boost::ref(get_step_plan_as)));
00037   stitch_step_plan_as = SimpleActionServer<msgs::StitchStepPlanAction>::create(nh, "stitch_step_plan", true, boost::bind(&GlobalFootstepPlannerNode::stitchStepPlanAction, this, boost::ref(stitch_step_plan_as)));
00038 }
00039 
00040 // --- Subscriber calls ---
00041 
00042 void GlobalFootstepPlannerNode::editStep(const msgs::EditStepConstPtr& edit_step)
00043 {
00044   std::vector<msgs::StepPlan> result;
00045   msgs::StepPlan step_plan;
00046   global_footstep_planner->getStepPlan(step_plan); // don't need to save status here
00047   msgs::ErrorStatus error_status = global_footstep_planner->editStep(*edit_step, step_plan, result);
00048 
00049   //step_plan_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(result)));
00050   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(error_status)));
00051 }
00052 
00053 void GlobalFootstepPlannerNode::setStepPlan(const msgs::StepPlanConstPtr& step_plan)
00054 {
00055   msgs::ErrorStatus error_status = global_footstep_planner->setStepPlan(*step_plan);
00056   msgs::StepPlan result;
00057   error_status += global_footstep_planner->getStepPlan(result);
00058 
00059   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(error_status)));
00060 }
00061 
00062 void GlobalFootstepPlannerNode::stitchStepPlan(const std::vector<msgs::StepPlan>& step_plans)
00063 {
00064   msgs::StepPlan result;
00065   msgs::ErrorStatus error_status = global_footstep_planner->stitchStepPlan(step_plans, result);
00066 
00067   step_plan_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(result)));
00068   step_plan_vis_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(result)));
00069   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(error_status)));
00070 }
00071 
00072 void GlobalFootstepPlannerNode::stitchStepPlan(const msgs::StepPlanConstPtr& step_plan)
00073 {
00074   std::vector<msgs::StepPlan> step_plans;
00075 
00076   msgs::StepPlan current_step_plan;
00077   global_footstep_planner->getStepPlan(current_step_plan);
00078   step_plans.push_back(current_step_plan);
00079   step_plans.push_back(*step_plan);
00080 
00081   stitchStepPlan(step_plans);
00082 }
00083 
00084 // --- Service calls ---
00085 
00086 bool GlobalFootstepPlannerNode::editStepService(msgs::EditStepService::Request &req, msgs::EditStepService::Response &resp)
00087 {
00088   resp.status = global_footstep_planner->editStep(req.edit_step, req.step_plan, resp.step_plans);
00089 
00090   //temp_step_plan_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(resp.step_plans)));
00091   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(resp.status)));
00092 
00093   return true; // return always true so the message is returned
00094 }
00095 
00096 bool GlobalFootstepPlannerNode::setStepPlanService(msgs::SetStepPlanService::Request &req, msgs::SetStepPlanService::Response &resp)
00097 {
00098   resp.status = global_footstep_planner->setStepPlan(req.step_plan);
00099   resp.status += global_footstep_planner->getStepPlan(resp.step_plan);
00100 
00101   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(resp.status)));
00102 
00103   return true; // return always true so the message is returned
00104 }
00105 
00106 bool GlobalFootstepPlannerNode::getStepPlanService(msgs::GetStepPlanService::Request &req, msgs::GetStepPlanService::Response &resp)
00107 {
00108   resp.status = global_footstep_planner->getStepPlan(resp.step_plan);
00109   return true; // return always true so the message is returned
00110 }
00111 
00112 bool GlobalFootstepPlannerNode::stitchStepPlanService(msgs::StitchStepPlanService::Request &req, msgs::StitchStepPlanService::Response &resp)
00113 {
00114   resp.status = global_footstep_planner->stitchStepPlan(req.step_plans, resp.step_plan);
00115 
00116   temp_step_plan_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(resp.step_plan)));
00117   step_plan_vis_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(resp.step_plan)));
00118   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(resp.status)));
00119 
00120   return true; // return always true so the message is returned
00121 }
00122 
00123 //--- action server calls ---
00124 
00125 void GlobalFootstepPlannerNode::editStepAction(SimpleActionServer<msgs::EditStepAction>::Ptr& as)
00126 {
00127   const msgs::EditStepGoalConstPtr& goal(as->acceptNewGoal());
00128 
00129   // check if new goal was preempted in the meantime
00130   if (as->isPreemptRequested())
00131   {
00132     as->setPreempted();
00133     return;
00134   }
00135 
00136   msgs::EditStepResult result;
00137   result.status = global_footstep_planner->editStep(goal->edit_step, goal->step_plan, result.step_plans);
00138 
00139   as->finish(result);
00140   //if (result.status.error == msgs::ErrorStatus::NO_ERROR)
00141  //   temp_step_plan_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(result.step_plan)));
00142   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(result.status)));
00143 }
00144 
00145 void GlobalFootstepPlannerNode::setStepPlanAction(SimpleActionServer<msgs::SetStepPlanAction>::Ptr& as)
00146 {
00147   const msgs::SetStepPlanGoalConstPtr& goal(as->acceptNewGoal());
00148 
00149   // check if new goal was preempted in the meantime
00150   if (as->isPreemptRequested())
00151   {
00152     as->setPreempted();
00153     return;
00154   }
00155 
00156   msgs::SetStepPlanResult result;
00157   result.status = global_footstep_planner->setStepPlan(goal->step_plan);
00158   result.status += global_footstep_planner->getStepPlan(result.step_plan);
00159 
00160   as->finish(result);
00161   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(result.status)));
00162 }
00163 
00164 void GlobalFootstepPlannerNode::getStepPlanAction(SimpleActionServer<msgs::GetStepPlanAction>::Ptr& as)
00165 {
00166   as->acceptNewGoal();
00167 
00168   // check if new goal was preempted in the meantime
00169   if (as->isPreemptRequested())
00170   {
00171     as->setPreempted();
00172     return;
00173   }
00174 
00175   msgs::GetStepPlanResult result;
00176   result.status = global_footstep_planner->getStepPlan(result.step_plan);
00177   as->setSucceeded(result, toString(result.status));
00178 }
00179 
00180 void GlobalFootstepPlannerNode::stitchStepPlanAction(SimpleActionServer<msgs::StitchStepPlanAction>::Ptr& as)
00181 {
00182   const msgs::StitchStepPlanGoalConstPtr& goal(as->acceptNewGoal());
00183 
00184   // check if new goal was preempted in the meantime
00185   if (as->isPreemptRequested())
00186   {
00187     as->setPreempted();
00188     return;
00189   }
00190 
00191   msgs::StitchStepPlanResult result;
00192   result.status = global_footstep_planner->stitchStepPlan(goal->step_plans, result.step_plan);
00193 
00194   as->finish(result);
00195   temp_step_plan_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(result.step_plan)));
00196   step_plan_vis_pub.publish(msgs::StepPlanConstPtr(new msgs::StepPlan(result.step_plan)));
00197   error_status_pub.publish(msgs::ErrorStatusConstPtr(new msgs::ErrorStatus(result.status)));
00198 }
00199 }
00200 
00201 int main(int argc, char **argv)
00202 {
00203   ros::init(argc, argv, "vigir_global_footstep_planner");
00204 
00205   ros::NodeHandle nh;
00206 
00207   // ensure that node's services are set up in proper namespace
00208   if (nh.getNamespace().size() <= 1)
00209     nh = ros::NodeHandle("~");
00210 
00211   // init parameter and plugin manager
00212   vigir_generic_params::ParameterManager::initialize(nh);
00213   vigir_pluginlib::PluginManager::initialize(nh);
00214 
00215   vigir_footstep_planning::GlobalFootstepPlannerNode globalPlannerNode(nh);
00216 
00217   ros::spin();
00218 
00219   return 0;
00220 }