action_test.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <actionlib/client/simple_action_client.h>
00003 #include <control_msgs/FollowJointTrajectoryAction.h>
00004 #include <control_msgs/FollowJointTrajectoryGoal.h>
00005 #include <sensor_msgs/JointState.h>
00006 #include <std_msgs/Float32MultiArray.h>
00007 #include <aubo_msgs/GoalPoint.h>
00008 
00009 using namespace std;
00010 int new_goal = 0;
00011 aubo_msgs::GoalPoint goalPos;
00012 std_msgs::Float32MultiArray curPos;
00013 
00014 typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
00015   
00016 class RobotArm
00017 {
00018     private:
00019      // Action client for the joint trajectory action 
00020      // used to trigger the arm movement action
00021      TrajClient* traj_client_;
00022    
00023    public:
00025      RobotArm() 
00026      {
00027        // tell the action client that we want to spin a thread by default
00028        traj_client_ = new TrajClient("follow_joint_trajectory", true);
00029    
00030        // wait for action server to come up
00031        while(!traj_client_->waitForServer(ros::Duration(5.0))){
00032          ROS_INFO("Waiting for the follow_joint_trajectory server");
00033        }
00034        
00035      }
00036    
00038      ~RobotArm()
00039      {
00040        delete traj_client_;
00041      }
00042    
00044      void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
00045      {
00046        traj_client_->sendGoal(goal);
00047        traj_client_->waitForResult();
00048      }
00049 
00050      control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory(aubo_msgs::GoalPoint &waypoints)
00051      {
00052          //our goal variable
00053          control_msgs::FollowJointTrajectoryGoal goal;
00054 
00055          // First, the joint names, which apply to all waypoints
00056          goal.trajectory.joint_names.push_back("shoulder_joint");
00057          goal.trajectory.joint_names.push_back("upperArm_joint");
00058          goal.trajectory.joint_names.push_back("foreArm_joint");
00059          goal.trajectory.joint_names.push_back("wrist1_joint");
00060          goal.trajectory.joint_names.push_back("wrist2_joint");
00061          goal.trajectory.joint_names.push_back("wrist3_joint");
00062 
00063 
00064          // We will have 2 waypoints in this goal trajectory
00065          goal.trajectory.points.resize(2);
00066 
00067          // Positions
00068          goal.trajectory.points[0].positions.resize(6);
00069          goal.trajectory.points[0].positions[0] = curPos.data[0];
00070          goal.trajectory.points[0].positions[1] = curPos.data[1];
00071          goal.trajectory.points[0].positions[2] = curPos.data[2];
00072          goal.trajectory.points[0].positions[3] = curPos.data[3];
00073          goal.trajectory.points[0].positions[4] = curPos.data[4];
00074          goal.trajectory.points[0].positions[5] = curPos.data[5];
00075 
00076          // Velocities
00077          goal.trajectory.points[0].velocities.resize(6);
00078          for (size_t j = 0; j < 6; ++j)
00079          {
00080            goal.trajectory.points[0].velocities[j] = 0.0;
00081          }
00082 
00083 
00084          // acceleration
00085          goal.trajectory.points[0].accelerations.resize(6);
00086          for (size_t j = 0; j < 6; ++j)
00087          {
00088            goal.trajectory.points[0].accelerations[j] = 0.0;
00089          }
00090 
00091          // To be reached 1 second after starting along the trajectory
00092          goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
00093 
00094 
00095 
00096          // Second trajectory point
00097          // Positions
00098          goal.trajectory.points[1].positions.resize(6);
00099 
00100          goal.trajectory.points[1].positions[0] = waypoints.joint1;
00101          goal.trajectory.points[1].positions[1] = waypoints.joint2;
00102          goal.trajectory.points[1].positions[2] = waypoints.joint3;
00103          goal.trajectory.points[1].positions[3] = waypoints.joint4;
00104          goal.trajectory.points[1].positions[4] = waypoints.joint5;
00105          goal.trajectory.points[1].positions[5] = waypoints.joint6;
00106 
00107 
00108          // Velocities
00109          goal.trajectory.points[1].velocities.resize(6);
00110          for (size_t j = 0; j < 6; ++j)
00111          {
00112            goal.trajectory.points[1].velocities[j] = 0.0;
00113          }
00114 
00115          // acceleration
00116          goal.trajectory.points[1].accelerations.resize(6);
00117          for (size_t j = 0; j < 6; ++j)
00118          {
00119            goal.trajectory.points[1].accelerations[j] = 0.0;
00120          }
00121 
00122          // To be reached 2 seconds after starting along the trajectory
00123          goal.trajectory.points[1].time_from_start = ros::Duration(2.5);
00124 
00125 
00126          return goal;
00127 
00128      }
00129 
00130   
00132     actionlib::SimpleClientGoalState getState()
00133     {
00134       return traj_client_->getState();
00135     }
00136 
00137   };
00138 
00139 
00140   void chatterCallback(const aubo_msgs::GoalPoint::ConstPtr &msg)
00141   {
00142     new_goal = 0;
00143 
00144     goalPos.bus =msg->bus;
00145     goalPos.joint1 = msg->joint1;
00146     goalPos.joint2 = msg->joint2;
00147     goalPos.joint3 = msg->joint3;
00148     goalPos.joint4 = msg->joint4;
00149     goalPos.joint5 = msg->joint5;
00150     goalPos.joint6 = msg->joint6;
00151     new_goal = 1;
00152     ROS_INFO("Goal");
00153   }
00154 
00155 
00156   void chatterCallback1(const sensor_msgs::JointState::ConstPtr &msg)
00157   {
00158     //ROS_INFO("Callback1[%f,%f,%f,%f,%f,%f]",msg->position[0],msg->position[1],msg->position[2],msg->position[3],msg->position[4],msg->position[5]);
00159     curPos.data[0] = msg->position[0];
00160     curPos.data[1] = msg->position[1];
00161     curPos.data[2] = msg->position[2];
00162     curPos.data[3] = msg->position[3];
00163     curPos.data[4] = msg->position[4];
00164     curPos.data[5] = msg->position[5];
00165   }
00166   
00167 
00168 
00169 
00170   int main(int argc, char** argv)
00171   {
00172     // Init the ROS node
00173     ros::init(argc, argv, "move_test");
00174     ros::NodeHandle nh;
00175 
00176     ros::Rate loop_rate(10);//Hz
00177 
00178     RobotArm arm;
00179 
00180     curPos.data.resize(6);
00181 
00182     ros::Subscriber sub = nh.subscribe("send_goal", 1000, chatterCallback);
00183     ros::Subscriber sub1 = nh.subscribe("joint_states", 1000, chatterCallback1);
00184 
00185     // Wait for trajectory completion
00186     while(ros::ok())
00187     {
00188         if(new_goal == 1)
00189         {
00190             new_goal = 0;
00191             arm.startTrajectory(arm.armExtensionTrajectory(goalPos));
00192         }
00193         loop_rate.sleep();
00194         ros::spinOnce();
00195     }
00196 
00197     return 0;
00198   }


aubo_new_driver
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:44