arm_pose_service.cpp

Go to the documentation of this file.

#include "move_arm.h"
#include <door_manipulation_tools/RobotPosture.h>
#include "switch_controller.h"

bool  serviceCallback(door_manipulation_tools::RobotPosture::Request  &req,
                      door_manipulation_tools::RobotPosture::Response &res )
{
  res.success = true;
  res.success = switch_controller("l_arm_controller", "l_arm_cart_imped_controller");
  if(!res.success){
    ROS_ERROR("Could not switch to l_arm_controller");
    return false;
  }
  res.success = switch_controller("r_arm_controller", "r_arm_cart_imped_controller");
  if(!res.success){
    ROS_ERROR("Could not switch to r_arm_controller");
    return false;
  }

  ROS_INFO("Starting Robot Arm");
  RobotArm robotArm;
  bool left_busy = false, right_busy = false;
  for(unsigned int i = 0; i < req.posture_names.size(); i++){
    ROS_INFO_STREAM("Moving to " << req.posture_names[i]);
    if(left_busy && req.posture_names[i][0] == 'l'){
        ROS_INFO_STREAM("Goal state: " << robotArm.getState(req.posture_names[i][0]).getText());
        left_busy = false;
    }
    if(right_busy && req.posture_names[i][0] == 'r'){
        ROS_INFO_STREAM("Goal state: " << robotArm.getState(req.posture_names[i][0]).getText());
        right_busy = false;
    }
    robotArm.startTrajectory(req.posture_names[i]);
    if(req.posture_names[i][0] == 'l')
      left_busy = true;
    else if(req.posture_names[i][0] == 'r')
      right_busy = true;
  }
  if(left_busy) ROS_INFO_STREAM("Goal state: " << robotArm.getState('l').getText());
  if(right_busy) ROS_INFO_STREAM("Goal state: " << robotArm.getState('r').getText());
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "robot_posture_service");
  ros::NodeHandle n;
  ros::ServiceServer service = n.advertiseService("robot_pose", serviceCallback);
  ROS_INFO("Robot pose service advertised. Waiting for clients.");
  ros::spin();
}