switch_controller.cpp

Go to the documentation of this file.

#include "switch_controller.h"
#include <ros/ros.h>
#include <pr2_mechanism_msgs/SwitchController.h>
#include <pr2_mechanism_msgs/LoadController.h>

bool load_controller(std::string name)
{
  return true;
  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<pr2_mechanism_msgs::LoadController>("/pr2_controller_manager/load_controller");

  pr2_mechanism_msgs::LoadController service_data;
  service_data.request.name = name;
  if (client.call(service_data)) { 
    ROS_INFO_STREAM("Loaded " << name); 
    return service_data.response.ok;
  } else { 
    ROS_INFO_STREAM("Failed to load " << name); 
    return false;
  } 
}
bool switch_controller(std::string start, std::string stop)
{
  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<pr2_mechanism_msgs::SwitchController>("/pr2_controller_manager/switch_controller");

  pr2_mechanism_msgs::SwitchController to_imped;
  to_imped.request.start_controllers.push_back(start);
  to_imped.request.stop_controllers.push_back(stop);
  to_imped.request.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
  if (client.call(to_imped)) { 
    ROS_INFO_STREAM("Switched from " << stop << " to "<<start); 
    return to_imped.response.ok;
  } else { 
    ROS_INFO_STREAM("Failed to switch from " << stop << " to "<<start); 
    return false;
  } 
}