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
00055 jt.points[i].time_from_start = ros::Duration(dt);
00056 ROS_INFO("test: angles[%d][%f, %f]",n,x1,x2);
00057 }
00058
00059
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);
00078
00079 return 0;
00080 }