#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACHnav_msgs::GetPlan srv;
ros::Publisher goal_pub;
ros::ServiceClient plan_client;void callPlanningService(ros::ServiceClient plan_client, nav_msgs::GetPlan &srv)
{if(plan_client.call(srv)){if(!srv.response.plan.poses.empty()){forEach(const geometry_msgs::PoseStamped &p, srv.response.plan.poses){ROS_INFO("x = %f, y =%f",p.pose.position.x, p.pose.position.y);}}else{ROS_INFO("EMPTY PLAN.");}}else{ROS_ERROR("Failed to call service.");}
}void clicked_point(const geometry_msgs::PointStamped& msg)
{//ROS_INFO("Got clicked point:[%f,%f,%f]", msg.pose.x, msg.pose.y, msg.pose.z);
nav_msgs::GetPlan srv;move_base_msgs::MoveBaseActionGoal goal;geometry_msgs::PoseStamped goalPos;goalPos.header.frame_id = "map";goalPos.header.stamp = ros::Time::now();goalPos.pose.position.x = 1.63;goalPos.pose.position.y = 2.9;goalPos.pose.position.z = 0;goalPos.pose.orientation.x = 0;goalPos.pose.orientation.y = 0;goalPos.pose.orientation.z = 0.115;goalPos.pose.orientation.w = 0.99;srv.request.start.header.stamp = ros::Time::now();srv.request.start.header.frame_id = "map";srv.request.start.pose.position.x  = msg.point.x;srv.request.start.pose.position.y  = msg.point.y;srv.request.start.pose.position.z  = 0;srv.request.start.pose.orientation.x  = 0;srv.request.start.pose.orientation.y  = 0;srv.request.start.pose.orientation.z  = 0;srv.request.start.pose.orientation.w  = 1;//srv.request.goal.header.frame_id = "map";srv.request.goal = goalPos;srv.request.tolerance = 0.5;goal.goal.target_pose = goalPos;goal_pub.publish(goal);callPlanningService(plan_client, srv);
}int main(int argc, char** argv)
{ros::init(argc, argv, "send_goal");ros::NodeHandle n;goal_pub=n.advertise<move_base_msgs::MoveBaseActionGoal>("/goal",10);plan_client=n.serviceClient<nav_msgs::GetPlan>("/move_base/make_plan");ros::Subscriber clickedpoint=n.subscribe("/clicked_point", 100, clicked_point);ros::spin();return 0;
}