pathPlannerClient.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <youbot_overhead_localization/PlanPathAction.h>
#include <youbot_overhead_localization/SetGoal.h>

class pathPlannerClient
{
public:
  actionlib::SimpleActionClient<youbot_overhead_localization::PlanPathAction> ac;
  ros::ServiceServer goalServer;
  ros::NodeHandle n;

  pathPlannerClient() :
      ac("path_planner", true)
  {
    ROS_INFO("Waiting for server...");
    ac.waitForServer();
    ROS_INFO("Done");
    ROS_INFO("Advertising service...");
    goalServer = n.advertiseService("update_goal", &pathPlannerClient::updateGoal, this);
    ROS_INFO("Done");
  }

  bool updateGoal(youbot_overhead_localization::SetGoal::Request &req,
                  youbot_overhead_localization::SetGoal::Response &res);
};

bool pathPlannerClient::updateGoal(youbot_overhead_localization::SetGoal::Request &req,
                                   youbot_overhead_localization::SetGoal::Response &res)
{
  //ac.cancelAllGoals();
  youbot_overhead_localization::PlanPathGoal goal;
  goal.topic_name = req.topic_name;
  goal.x = req.x;
  goal.y = req.y;
  ac.sendGoal(goal);

  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "path_planner_client");

  pathPlannerClient pathClient;

  ros::spin();
}