pub_joint_trajectory_test.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <trajectory_msgs/JointTrajectory.h>
00003 #include <gazebo_msgs/SetJointTrajectory.h>
00004 #include <gazebo/math/Quaternion.hh>
00005 #include <math.h>
00006 
00007 int main(int argc, char** argv)
00008 {
00009 
00010   ros::init(argc, argv, "pub_joint_trajectory_test");
00011   ros::NodeHandle rosnode;
00012   ros::Publisher pub_ = rosnode.advertise<trajectory_msgs::JointTrajectory>("set_joint_trajectory",100);
00013   ros::ServiceClient srv_ = rosnode.serviceClient<gazebo_msgs::SetJointTrajectory>("set_joint_trajectory");
00014 
00015   trajectory_msgs::JointTrajectory jt;
00016 
00017   jt.header.stamp = ros::Time::now();
00018   jt.header.frame_id = "pr2::torso_lift_link";
00019 
00020   jt.joint_names.push_back("r_shoulder_pan_joint");
00021   jt.joint_names.push_back("r_shoulder_lift_joint");
00022   jt.joint_names.push_back("r_upper_arm_roll_joint");
00023   jt.joint_names.push_back("r_elbow_flex_joint");
00024   jt.joint_names.push_back("r_forearm_roll_joint");
00025   jt.joint_names.push_back("r_wrist_flex_joint");
00026   jt.joint_names.push_back("l_shoulder_pan_joint");
00027   jt.joint_names.push_back("l_shoulder_lift_joint");
00028   jt.joint_names.push_back("l_upper_arm_roll_joint");
00029   jt.joint_names.push_back("l_elbow_flex_joint");
00030   jt.joint_names.push_back("l_forearm_roll_joint");
00031   jt.joint_names.push_back("l_wrist_flex_joint");
00032 
00033   int n = 500;
00034   double dt = 0.1;
00035   double rps = 0.05;
00036   jt.points.resize(n);
00037   for (int i = 0; i < n; i++)
00038   {
00039     double theta = rps*2.0*M_PI*i*dt;
00040     double x1 = -0.5*sin(2*theta);
00041     double x2 =  0.5*sin(1*theta);
00042     jt.points[i].positions.push_back(x1);
00043     jt.points[i].positions.push_back(x2);
00044     jt.points[i].positions.push_back(3.14);
00045     jt.points[i].positions.push_back(-10.0);
00046     jt.points[i].positions.push_back(-0.2);
00047     jt.points[i].positions.push_back(-0.2);
00048     jt.points[i].positions.push_back(x1);
00049     jt.points[i].positions.push_back(x2);
00050     jt.points[i].positions.push_back(3.14);
00051     jt.points[i].positions.push_back(10.0);
00052     jt.points[i].positions.push_back(-0.2);
00053     jt.points[i].positions.push_back(-0.2);
00054     // set duration
00055     jt.points[i].time_from_start = ros::Duration(dt);
00056     ROS_INFO("test: angles[%d][%f, %f]",n,x1,x2);
00057   }
00058 
00059   // pub_.publish(jt); // use publisher
00060 
00061   gazebo_msgs::SetJointTrajectory sjt;
00062   sjt.request.joint_trajectory = jt;
00063   sjt.request.disable_physics_updates = false;
00064 
00065   gazebo::math::Quaternion r(0, 0, M_PI);
00066   geometry_msgs::Pose p;
00067   p.position.x = 0;
00068   p.position.y = 0;
00069   p.position.z = 0;
00070   p.orientation.x = r.x;
00071   p.orientation.y = r.y;
00072   p.orientation.z = r.z;
00073   p.orientation.w = r.w;
00074   sjt.request.model_pose = p;
00075   sjt.request.set_model_pose = true;
00076 
00077   srv_.call(sjt); // use service
00078 
00079   return 0;
00080 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09