test_arm_trajectory_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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   /*********** Initialize ROS  ****************/
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   /*********** Start moving the robot ************/
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   cmd.points[0].positions[0] = 0.0;
00076   cmd.points[0].positions[1] = 0.0;
00077   cmd.points[0].positions[2] = 0.0;
00078   cmd.points[0].positions[3] = 0.0;
00079   cmd.points[0].positions[4] = 0.0;
00080   cmd.points[0].positions[5] = 0.0;
00081   cmd.points[0].positions[6] = 0.0;
00082   cmd.points[0].time = 0.0;
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 }


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Apr 24 2014 15:44:51