/opt/ros/diamondback/stacks/wg_robots_gazebo/pr2_arm_gazebo/src/move_arm.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <gazebo/ApplyBodyWrench.h>

typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;

class RobotArm
{
private:
  // Action client for the joint trajectory action 
  // used to trigger the arm movement action
  TrajClient* traj_client_;

public:
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);

    // wait for action server to come up
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  ~RobotArm()
  {
    delete traj_client_;
  }

  void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 1s from now
    //goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(3.0);
    ros::Duration wait_time(20.0);
    actionlib::SimpleClientGoalState state = traj_client_->sendGoalAndWait(goal,wait_time,wait_time);
    if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The arm traj finished with state [%s]",state.toString().c_str());
    else
      ROS_INFO("The arm traj failed with state [%s]",state.toString().c_str());
  }


  pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
  {
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(2);

    // First trajectory point
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = 0.0;
    goal.trajectory.points[ind].positions[1] = 0.0;
    goal.trajectory.points[ind].positions[2] = 0.0;
    goal.trajectory.points[ind].positions[3] = 0.0;
    goal.trajectory.points[ind].positions[4] = 0.0;
    goal.trajectory.points[ind].positions[5] = 0.0;
    goal.trajectory.points[ind].positions[6] = 0.0;
    // To be reached 1 second after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(3.0);

    // Second trajectory point
    // Positions
    ind += 1;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = 0.0;
    goal.trajectory.points[ind].positions[1] = 0.25;
    goal.trajectory.points[ind].positions[2] = 0.0;
    goal.trajectory.points[ind].positions[3] = 0.0;
    goal.trajectory.points[ind].positions[4] = 0.0;
    goal.trajectory.points[ind].positions[5] = 0.0;
    goal.trajectory.points[ind].positions[6] = 0.0;
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(6.0);

    //we are done; return the goal
    return goal;
  }

  actionlib::SimpleClientGoalState getState()
  {
    return traj_client_->getState();
  }
 
};

// Our Action interface type, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

class Gripper{
private:
  GripperClient* gripper_client_;  

public:
  //Action client initialization
  Gripper(){

    //Initialize the client for the Action interface to the gripper controller
    //and tell the action client that we want to spin a thread by default
    gripper_client_ = new GripperClient("r_gripper_controller/gripper_action", true);
    
    //wait for the gripper action server to come up 
    while(!gripper_client_->waitForServer(ros::Duration(30.0))){
      ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
    }
  }

  ~Gripper(){
    delete gripper_client_;
  }

  //Open the gripper
  void open(){
    pr2_controllers_msgs::Pr2GripperCommandGoal open;
    open.command.position = 0.08;
    open.command.max_effort = 100.0;  // use -1 to not limit effort (negative)
    
    ROS_INFO("Sending open goal");
    if (gripper_client_->sendGoalAndWait(open,ros::Duration(10.0),ros::Duration(5.0)) == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper opened!");
    else
      ROS_INFO("The gripper failed to open.");
  }

  //Close the gripper
  void close(){
    pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
    squeeze.command.position = 0.0;
    squeeze.command.max_effort = 200.0;  // Close gently
    
    ROS_INFO("Sending squeeze goal");
    if (gripper_client_->sendGoalAndWait(squeeze,ros::Duration(10.0),ros::Duration(5.0)) == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper closed!");
    else
      ROS_INFO("The gripper failed to close.");
    gripper_client_->sendGoal(squeeze);
  }
};

int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "robot_driver");
  ros::NodeHandle rh("");
  ros::Duration(1e-9).sleep();

  Gripper gripper;
  RobotArm arm;

  // open gripper
  gripper.open();

  // Start the trajectory
  arm.startTrajectory(arm.armExtensionTrajectory());

  //sleep(5); // FIXME: seems startTrajectory finishes without waiting?

  // close gripper
  gripper.close();

  // pull object down
  ros::ServiceClient client = rh.serviceClient<gazebo::ApplyBodyWrench>("/gazebo/apply_body_wrench");
  gazebo::ApplyBodyWrench wrench;
  //wrench.request.body_name       = "object_1::object_1_base_link";
  //wrench.request.reference_frame = "object_1::object_1_base_link";
  wrench.request.body_name       = "object_1::coke_can";
  wrench.request.reference_frame = "world";
  wrench.request.wrench.force.z = -20.0;
  wrench.request.start_time = ros::Time(0.0);
  wrench.request.duration = ros::Duration(-1.0);
  client.call(wrench);



}

---

pr2_arm_gazebo
Author(s): John Hsu
autogenerated on Wed Aug 17 09:00:11 2011