Go to the documentation of this file.00001 #include <vigir_feet_pose_generator/feet_pose_generator_node.h>
00002
00003 namespace vigir_footstep_planning
00004 {
00005 FeetPoseGeneratorNode::FeetPoseGeneratorNode(ros::NodeHandle& nh)
00006 : feet_pose_generator(nh)
00007 {
00008
00009 robot_pose_sub = nh.subscribe("/robot_pose", 1, &FeetPoseGenerator::setRobotPose, &feet_pose_generator);
00010 robot_pose_with_cov_sub = nh.subscribe("/robot_pose_with_cov", 1, &FeetPoseGenerator::setRobotPoseWithCovariance, &feet_pose_generator);
00011 terrain_model_sub = nh.subscribe("/terrain_model", 1, &FeetPoseGenerator::setTerrainModel, &feet_pose_generator);
00012
00013
00014 generate_feet_pose_srv = nh.advertiseService("generate_feet_pose", &FeetPoseGeneratorNode::generateFeetPoseService, this);
00015
00016
00017 generate_feet_pose_as = SimpleActionServer<msgs::GenerateFeetPoseAction>::create(nh, "generate_feet_pose", true, boost::bind(&FeetPoseGeneratorNode::generateFeetPoseAction, this, boost::ref(generate_feet_pose_as)));
00018 }
00019
00020 FeetPoseGeneratorNode::~FeetPoseGeneratorNode()
00021 {
00022 }
00023
00024
00025
00026 bool FeetPoseGeneratorNode::generateFeetPoseService(msgs::GenerateFeetPoseService::Request& req, msgs::GenerateFeetPoseService::Response& resp)
00027 {
00028 resp.status = feet_pose_generator.generateFeetPose(req.request, resp.feet);
00029 return true;
00030 }
00031
00032
00033
00034 void FeetPoseGeneratorNode::generateFeetPoseAction(SimpleActionServer<msgs::GenerateFeetPoseAction>::Ptr& as)
00035 {
00036 const msgs::GenerateFeetPoseGoalConstPtr& goal(as->acceptNewGoal());
00037
00038
00039 if (as->isPreemptRequested())
00040 {
00041 as->setPreempted();
00042 return;
00043 }
00044
00045 msgs::GenerateFeetPoseResult result;
00046 result.header = goal->request.header;
00047 result.status = feet_pose_generator.generateFeetPose(goal->request, result.feet);
00048
00049 as->finish(result);
00050 }
00051 }
00052
00053 int main(int argc, char **argv)
00054 {
00055 ros::init(argc, argv, "feet_pose_generator");
00056 ros::NodeHandle nh;
00057 vigir_footstep_planning::FeetPoseGeneratorNode generator(nh);
00058 ros::spin();
00059
00060 return 0;
00061 }