Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <ros/node.h>
00036 #include <manipulation_msgs/JointTraj.h>
00037
00038 static int done = 0;
00039
00040 void finalize(int donecare)
00041 {
00042 done = 1;
00043 }
00044
00045 int main( int argc, char** argv )
00046 {
00047
00048
00049 ros::init(argc,argv);
00050 ros::Node *node = new ros::Node("test_arm_trajectory_controller");
00051
00052 signal(SIGINT, finalize);
00053 signal(SIGQUIT, finalize);
00054 signal(SIGTERM, finalize);
00055
00056
00057
00058 manipulation_msgs::JointTraj cmd;
00059
00060 int num_points = 3;
00061 int num_joints = 14;
00062
00063 cmd.set_points_size(num_points);
00064
00065 for(int i=0; i<num_points; i++)
00066 {
00067 cmd.points[i].set_positions_size(num_joints);
00068 for(int j=0; j < num_joints; j++)
00069 {
00070 cmd.points[i].positions[j] = 0.0;
00071 }
00072 }
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 cmd.points[0].positions[0] = 0.5;
00086 cmd.points[0].positions[1] = 0.5;
00087 cmd.points[0].positions[2] = 0.2;
00088 cmd.points[0].positions[3] = -0.5;
00089 cmd.points[0].positions[4] = 0.4;
00090 cmd.points[0].positions[5] = 0.0;
00091 cmd.points[0].positions[6] = 0.0;
00092 cmd.points[0].time = 0.0;
00093
00094 cmd.points[1].positions[0] = 0.0;
00095 cmd.points[1].positions[1] = 0.0;
00096 cmd.points[1].positions[2] = 0.0;
00097 cmd.points[1].positions[3] = 0.0;
00098 cmd.points[1].positions[4] = 0.0;
00099 cmd.points[1].positions[5] = 0.0;
00100 cmd.points[1].positions[6] = 0.0;
00101 cmd.points[1].time = 0.0;
00102
00103 cmd.points[2].positions[0] = -0.5;
00104 cmd.points[2].positions[1] = 0.3;
00105 cmd.points[2].positions[2] = 0.2;
00106 cmd.points[2].positions[3] = -1.0;
00107 cmd.points[2].positions[4] = -0.4;
00108 cmd.points[2].positions[5] = 0.0;
00109 cmd.points[2].positions[6] = 0.0;
00110 cmd.points[2].time = 0.0;
00111
00112 node->advertise<manipulation_msgs::JointTraj>("/arm/trajectory_controller/arm_trajectory_command",1);
00113 node->publish("/arm/trajectory_controller/arm_trajectory_command",cmd);
00114 sleep(4);
00115
00116 ros::Time start_time = ros::Time::now();
00117 ros::Duration sleep_time = ros::Duration().fromSec(0.01);
00118
00119 }