trajectory_client.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 <std_msgs/Float32MultiArray.h>
00005 
00006 using namespace std;
00007 int new_goal = 0;
00008 std_msgs::Float32MultiArray goal;
00009 double last_way_point[6];
00010  
00011 typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
00012   
00013 class RobotArm
00014 {
00015     private:
00016      // Action client for the joint trajectory action 
00017      // used to trigger the arm movement action
00018      TrajClient* traj_client_;
00019    
00020    public:
00022      RobotArm() 
00023      {
00024        // tell the action client that we want to spin a thread by default
00025        traj_client_ = new TrajClient("arm_controller/follow_joint_trajectory", true);
00026    
00027        // wait for action server to come up
00028        while(!traj_client_->waitForServer(ros::Duration(5.0))){
00029          ROS_INFO("Waiting for the follow_joint_trajectory server");
00030        }
00031        
00032      }
00033    
00035      ~RobotArm()
00036      {
00037        delete traj_client_;
00038      }
00039    
00041      void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
00042      {
00043        // When to start the trajectory: 1s from now
00044        goal.trajectory.header.frame_id = "base_Link";  
00045        goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00046        traj_client_->sendGoal(goal);
00047      }
00048 
00049      control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory(std_msgs::Float32MultiArray &waypoints)
00050      {
00051          //our goal variable
00052          control_msgs::FollowJointTrajectoryGoal goal;
00053 
00054          // First, the joint names, which apply to all waypoints
00055          goal.trajectory.joint_names.push_back("shoulder_joint");
00056          goal.trajectory.joint_names.push_back("upperArm_joint");
00057          goal.trajectory.joint_names.push_back("foreArm_joint");
00058          goal.trajectory.joint_names.push_back("wrist1_joint");
00059          goal.trajectory.joint_names.push_back("wrist2_joint");
00060          goal.trajectory.joint_names.push_back("wrist3_joint");
00061 
00062          // We will have 1 waypoints in this goal trajectory
00063          goal.trajectory.points.resize(1);
00064 
00065 
00066          // Positions
00067          goal.trajectory.points[0].positions.resize(6);
00068          goal.trajectory.points[0].positions[0] = waypoints.data[0];
00069          goal.trajectory.points[0].positions[1] = waypoints.data[1];
00070          goal.trajectory.points[0].positions[2] = waypoints.data[2];
00071          goal.trajectory.points[0].positions[3] = waypoints.data[3];
00072          goal.trajectory.points[0].positions[4] = waypoints.data[4];
00073          goal.trajectory.points[0].positions[5] = waypoints.data[5];
00074 
00075          // Velocities
00076          goal.trajectory.points[0].velocities.resize(6);
00077          for (size_t j = 0; j < 6; ++j)
00078          {
00079            goal.trajectory.points[0].velocities[j] = 0.001;
00080          }
00081 
00082 
00083          // acceleration
00084          goal.trajectory.points[0].accelerations.resize(6);
00085          for (size_t j = 0; j < 6; ++j)
00086          {
00087            goal.trajectory.points[0].accelerations[j] = 0.0;
00088          }
00089 
00090          // To be reached 1 second after starting along the trajectory
00091          goal.trajectory.points[0].time_from_start = ros::Duration(1);
00092 
00093          return goal;
00094 
00095      }
00096 
00097    
00099 
00104      control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory()
00105      {
00106 
00107        //our goal variable
00108        control_msgs::FollowJointTrajectoryGoal goal;
00109    
00110        // First, the joint names, which apply to all waypoints
00111        goal.trajectory.joint_names.push_back("shoulder_joint");
00112        goal.trajectory.joint_names.push_back("upperArm_joint");
00113        goal.trajectory.joint_names.push_back("foreArm_joint");
00114        goal.trajectory.joint_names.push_back("wrist1_joint");
00115        goal.trajectory.joint_names.push_back("wrist2_joint");
00116        goal.trajectory.joint_names.push_back("wrist3_joint");
00117 
00118 
00119        // We will have 1 waypoints in this goal trajectory
00120        goal.trajectory.points.resize(2);
00121 
00122    
00123        // First trajectory point
00124        // Positions
00125        int ind = 0;
00126        goal.trajectory.points[ind].positions.resize(6);
00127        goal.trajectory.points[ind].positions[0] = 0;
00128        goal.trajectory.points[ind].positions[1] = 0;
00129        goal.trajectory.points[ind].positions[2] = 0;
00130        goal.trajectory.points[ind].positions[3] = 0;
00131        goal.trajectory.points[ind].positions[4] = 0;
00132        goal.trajectory.points[ind].positions[5] = 0;
00133 
00134        // Velocities
00135        goal.trajectory.points[ind].velocities.resize(6);
00136        for (size_t j = 0; j < 6; ++j)
00137        {
00138          goal.trajectory.points[ind].velocities[j] = 0.0;
00139        }
00140 
00141 
00142        // acceleration
00143        goal.trajectory.points[ind].accelerations.resize(6);
00144        for (size_t j = 0; j < 6; ++j)
00145        {
00146          goal.trajectory.points[ind].accelerations[j] = 0.0;
00147        }
00148 
00149 
00150        // To be reached 1 second after starting along the trajectory
00151        goal.trajectory.points[ind].time_from_start = ros::Duration(1);
00152 
00153 
00154        // Second trajectory point
00155        // Positions
00156        ind += 1;
00157        goal.trajectory.points[ind].positions.resize(6);
00158 
00159        goal.trajectory.points[ind].positions[0] = -2.33;
00160        goal.trajectory.points[ind].positions[1] = 1.38;
00161        goal.trajectory.points[ind].positions[2] = 0.97;
00162        goal.trajectory.points[ind].positions[3] = 0.43;
00163        goal.trajectory.points[ind].positions[4] = -1.01;
00164        goal.trajectory.points[ind].positions[5] = 0.01;
00165 
00166        last_way_point[0] = -2.33;
00167        last_way_point[1] = 1.38;
00168        last_way_point[2] = 0.97;
00169        last_way_point[3] = 0.43;
00170        last_way_point[4] = -1.01;
00171        last_way_point[5] = 0.01;
00172 
00173   
00174        // Velocities
00175        goal.trajectory.points[ind].velocities.resize(6);
00176        for (size_t j = 0; j < 6; ++j)
00177        {
00178          goal.trajectory.points[ind].velocities[j] = 0.0;
00179        }
00180 
00181        // acceleration
00182        goal.trajectory.points[ind].accelerations.resize(6);
00183        for (size_t j = 0; j < 6; ++j)
00184        {
00185          goal.trajectory.points[ind].accelerations[j] = 0.0;
00186        }
00187 
00188        // To be reached 2 seconds after starting along the trajectory
00189        goal.trajectory.points[ind].time_from_start = ros::Duration(2);
00190 
00191        //we are done; return the goal
00192        return goal;
00193     }
00194   
00196     actionlib::SimpleClientGoalState getState()
00197     {
00198       return traj_client_->getState();
00199     }
00200   };
00201 
00202  int road_point_compare(double *goal)
00203  {
00204   int ret = 0;
00205   for(int i=0;i<6;i++)
00206   {
00207     if(fabs(goal[i]-last_way_point[i])>=0.000001)
00208     {
00209        ret = 1;
00210        break;
00211     }
00212   }
00213 
00214   if(ret == 1)
00215   {
00216     last_way_point[0]= goal[0];
00217     last_way_point[1]= goal[1];
00218     last_way_point[2]= goal[2];
00219     last_way_point[3]= goal[3];
00220     last_way_point[4]= goal[4];
00221     last_way_point[5]= goal[5];
00222   }
00223 
00224 
00225   return ret;
00226  }
00227 
00228   void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
00229   {
00230     double road[6];
00231     new_goal = 0;
00232     road[0] = msg->data[0];
00233     road[1] = msg->data[1];
00234     road[2] = msg->data[2];
00235     road[3] = msg->data[3];
00236     road[4] = msg->data[4];
00237     road[5] = msg->data[5];
00238 
00239     if(road_point_compare(road))
00240     {
00241         goal.data[0] = road[0];
00242         goal.data[1] = road[1];
00243         goal.data[2] = road[2];
00244         goal.data[3] = road[3];
00245         goal.data[4] = road[4];
00246         goal.data[5] = road[5];
00247         new_goal = 1;
00248     }
00249   }
00250   
00251   int main(int argc, char** argv)
00252   {
00253     // Init the ROS node
00254     ros::init(argc, argv, "trajectory_client");
00255     ros::NodeHandle nh;
00256 
00257     ros::Rate loop_rate(20);//Hz
00258 
00259     RobotArm arm;
00260 
00261     goal.data.resize(6);
00262 
00263     ros::Subscriber sub = nh.subscribe("send_goal", 1000, chatterCallback);
00264     arm.startTrajectory(arm.armExtensionTrajectory());
00265 
00266     // Wait for trajectory completion
00267     while(ros::ok())
00268     {
00269         if(new_goal)
00270         {
00271             new_goal = 0;
00272             if(arm.getState().isDone())
00273             {
00274                 arm.startTrajectory(arm.armExtensionTrajectory(goal));
00275             }
00276         }
00277         loop_rate.sleep();
00278         ros::spinOnce();
00279     }
00280 
00281     return 0;
00282   }


aubo_trajectory
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:56