trajectory_goal.cpp
Go to the documentation of this file.
00001 
00002 #include "ros/ros.h"
00003 #include "std_msgs/Float32MultiArray.h"
00004 #include "aubo_msgs/GoalPoint.h"
00005 #include "aubo_msgs/TraPoint.h"
00006 
00007 aubo_msgs::GoalPoint goal;
00008 ros::Publisher goal_pub;
00009 
00010 
00011 std_msgs::Float32MultiArray joints_cmd;
00012 ros::Publisher cmd_pub;
00013 ros::Publisher cmd_pub2;
00014 
00015  
00016 aubo_msgs::TraPoint traj_point;
00017 
00018 bool recv_flag = false;
00019 ros::Timer timer;
00020 
00021 void chatterCallback(const aubo_msgs::TraPoint::ConstPtr &msg)
00022 {
00023   traj_point.bus = msg->bus;
00024 
00025   traj_point.num_of_trapoint = msg->num_of_trapoint;
00026   traj_point.trapoints.resize(msg->num_of_trapoint*6);
00027 
00028   for(int i = 0;i<msg->num_of_trapoint;i++)
00029   {
00030      for(int j=0;j<6;j++)
00031      {
00032        traj_point.trapoints[i*6+j]=msg->trapoints[i*6+j];
00033      }
00034   }
00035 
00036   ROS_INFO("point:%d",traj_point.num_of_trapoint);
00037   recv_flag = true;
00038 
00039   timer.start();
00040 }
00041 
00042 
00043 void chatterCallback1(const aubo_msgs::GoalPoint::ConstPtr &msg)
00044 {
00045     //ROS_INFO("goal=[%f,%f,%f,%f,%f,%f]",msg->joint1,msg->joint2,msg->joint3,msg->joint4,msg->joint5,msg->joint6);
00046 
00047     goal.bus = msg->bus;
00048     goal.joint1 = msg->joint1;
00049     goal.joint2 = msg->joint2;
00050     goal.joint3 = msg->joint3;
00051     goal.joint4 = msg->joint4;
00052     goal.joint5 = msg->joint5;
00053     goal.joint6 = msg->joint6;
00054     goal_pub.publish(goal);
00055 }
00056 
00057 
00058 void timerCallback(const ros::TimerEvent& e)
00059 {
00060     static int i=0;
00061 
00062     if(recv_flag == true)
00063     { 
00064          if((i>=traj_point.num_of_trapoint)&&(traj_point.num_of_trapoint>0))
00065          {
00066             i = 0;
00067             traj_point.num_of_trapoint = 0;
00068             recv_flag = false;
00069             timer.stop();
00070          }
00071 
00072          
00073          if(recv_flag == true)
00074          {
00075            for(int j=0;j<6;j++)
00076            {
00077             joints_cmd.data[j] = traj_point.trapoints[i*6+j];
00078            }
00079 
00080            if(traj_point.bus == 0)
00081            {
00082                cmd_pub.publish(joints_cmd);
00083                //ROS_INFO("movej:%f,%f,%f,%f,%f,%f",joints_cmd.data[0],joints_cmd.data[1],joints_cmd.data[2],joints_cmd.data[3],joints_cmd.data[4],joints_cmd.data[5]);
00084 
00085            }
00086            else if(traj_point.bus == 1 )
00087            {
00088                cmd_pub2.publish(joints_cmd);
00089                //ROS_INFO("servoj:%f,%f,%f,%f,%f,%f",joints_cmd.data[0],joints_cmd.data[1],joints_cmd.data[2],joints_cmd.data[3],joints_cmd.data[4],joints_cmd.data[5]);
00090            }
00091 
00092            i++;
00093          }  
00094    }
00095    else
00096    {
00097       timer.stop();
00098    }
00099 
00100 }
00101 
00102 
00103 int main(int argc, char **argv)
00104 {
00105     ros::init(argc, argv, "trajectory_goal");
00106     ros::NodeHandle nh;
00107 
00108     ros::AsyncSpinner spinner(6);
00109     spinner.start();
00110  
00111     goal_pub = nh.advertise<aubo_msgs::GoalPoint> ("goal_pos", 1);
00112 
00113     cmd_pub = nh.advertise<std_msgs::Float32MultiArray> ("pcan_cmd", 1);
00114     cmd_pub2 = nh.advertise<std_msgs::Float32MultiArray> ("servoj_cmd", 1);
00115 
00116     ros::Subscriber sub = nh.subscribe("tra_point", 1000, chatterCallback);
00117     ros::Subscriber sub1 = nh.subscribe("send_goal", 1000, chatterCallback1);
00118 
00119     timer = nh.createTimer(ros::Duration(0.01),timerCallback);
00120     timer.stop();
00121   
00122     joints_cmd.data.resize(6);
00123 
00124     sleep(2.0);
00125 
00126     ros::waitForShutdown(); 
00127 
00128     return 0;
00129 }
00130 


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