test.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>

#include <move_arm_msgs/MoveArmAction.h>
#include <move_arm_msgs/utils.h>

#include <nav_msgs/Odometry.h>
#include <boost/thread.hpp>

#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
#include <tf/transform_listener.h>


// for opening gripper
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <actionlib/client/simple_action_client.h>

// 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(5.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 = -1.0;  // Do not limit effort (negative)
    
    ROS_INFO("Sending open goal");
    gripper_client_->sendGoal(open);
    gripper_client_->waitForResult();
    if(gripper_client_->getState() == 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 = 50.0;  // Close gently
    
    ROS_INFO("Sending squeeze goal, no block, just continue");
    gripper_client_->sendGoal(squeeze);
/*
    ROS_INFO("Sending squeeze goal, wait for 10 seconds, check for stall, then continue");
    if(gripper_client_->sendGoalAndWait(squeeze,ros::Duration(10.0), ros::Duration(5.0)) == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper closed!");
    else
    {
      if (gripper_client_->getResult()->stalled)
        ROS_INFO("The gripper failed to close and is stalled, possibly holding an object.");
      else
        ROS_INFO("The gripper failed to close and is not stalled, something is wrong.");
    }
*/
  }
};

class objectPose
{
    public:
      bool got_pose_;
      nav_msgs::Odometry pose_;
      ros::Subscriber sub_;

      objectPose(ros::NodeHandle& nh,std::string topic)
      {
        this->got_pose_ = false;
        this->sub_ =  nh.subscribe(topic, 1, &objectPose::poseCallback, this);
      };
      void poseCallback(const nav_msgs::OdometryConstPtr& pose)
      {
         this->pose_ = *pose;
         this->got_pose_ = true;
      };
};


int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_pose_goal_test");
  ros::NodeHandle nh;

  // open gripper
  //Gripper gripper;
  //gripper.close();

  // spin some threaded spinners
  ros::MultiThreadedSpinner s(4);
  boost::thread* ros_spinner = new boost::thread(boost::bind(&ros::spin,s));






  // offset pose by transform from r_wrist_roll_link to r_gripper_tool_frame
  // get the fixed transform from r_wrist_roll_link to r_gripper_tool_frame
  std::string target_frame = "r_gripper_tool_frame";
  std::string command_link = "r_wrist_roll_link";
  ROS_INFO("waiting for transform from %s to %s",command_link.c_str(),target_frame.c_str());
  tf::TransformListener listener;
  tf::StampedTransform target_to_command_link; // tool frame in wrist_tool_link frame
  listener.waitForTransform(target_frame,command_link,ros::Time(),ros::Duration(100.0));
  listener.lookupTransform(target_frame,command_link,ros::Time(0),target_to_command_link);



  // getting outlet pose from sim
  objectPose ocb(nh,"/outlet/outlet_pose_ground_truth");
  ROS_INFO("waiting for /outlet/outlet_pose_ground_truth from sim");
  while (!ocb.got_pose_)
  {
    ROS_DEBUG("checking for outlet pose (%d)",  ocb.got_pose_ );
    ros::Duration(1.0).sleep();
  }
  // get reference frame of outlet_pose
  std::string outlet_reference_frame = ocb.pose_.header.frame_id;

  // set goal message
  btTransform outlet_transform(btQuaternion(ocb.pose_.pose.pose.orientation.x,
                                            ocb.pose_.pose.pose.orientation.y,
                                            ocb.pose_.pose.pose.orientation.z,
                                            ocb.pose_.pose.pose.orientation.w),
                               btVector3(ocb.pose_.pose.pose.position.x,
                                         ocb.pose_.pose.pose.position.y,
                                         ocb.pose_.pose.pose.position.z));
  //outlet_transform =  outlet_transform*(btTransform)target_to_command_link; 


  /*****************************************************/
  /*                                                   */
  /*   goalC: goal pose                                */
  /*                                                   */
  /*****************************************************/
  actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);
  ROS_INFO("Connecting to server");
  move_arm.waitForServer();

  move_arm_msgs::MoveArmGoal goalC;

  goalC.group_name = "right_arm";
  goalC.num_planning_attempts = 1;
  goalC.planner_id = std::string("");
  goalC.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalC.allowed_planning_time = 5.0;
  
  motion_planning_msgs::SimplePoseConstraint desired_poseC;

  desired_poseC.header = ocb.pose_.header;
  desired_poseC.link_name = command_link;
  desired_poseC.pose.position.x = outlet_transform.getOrigin().x()-0.30;
  desired_poseC.pose.position.y = outlet_transform.getOrigin().y();
  desired_poseC.pose.position.z = outlet_transform.getOrigin().z();
  desired_poseC.pose.orientation.x = outlet_transform.getRotation().x();
  desired_poseC.pose.orientation.y = outlet_transform.getRotation().y();
  desired_poseC.pose.orientation.z = outlet_transform.getRotation().z();
  desired_poseC.pose.orientation.w = outlet_transform.getRotation().w();

  std::cout << "pose " << std::endl
            << "frame " << desired_poseC.header.frame_id << std::endl
            << ", " << desired_poseC.pose.position.x
            << ", " << desired_poseC.pose.position.y
            << ", " << desired_poseC.pose.position.z
            << ", " << desired_poseC.pose.orientation.x
            << ", " << desired_poseC.pose.orientation.y
            << ", " << desired_poseC.pose.orientation.z
            << ", " << desired_poseC.pose.orientation.w
            << std::endl;

  // additional parameters
  desired_poseC.absolute_position_tolerance.x = 0.02;
  desired_poseC.absolute_position_tolerance.y = 0.02;
  desired_poseC.absolute_position_tolerance.z = 0.02;

  desired_poseC.absolute_roll_tolerance = 0.04;
  desired_poseC.absolute_pitch_tolerance = 0.04;
  desired_poseC.absolute_yaw_tolerance = 0.04;
  
  ROS_INFO("sending final pose goal to server");
  move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_poseC,goalC);

  // add another goal

  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalC);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal C");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }

  ros::shutdown();
  ros_spinner->join();
}