pub_joint_trajectory_test.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <trajectory_msgs/JointTrajectory.h>
3 #include <gazebo_msgs/SetJointTrajectory.h>
4 #include <ignition/math/Quaternion.hh>
5 #include <math.h>
6 
7 int main(int argc, char** argv)
8 {
9 
10  ros::init(argc, argv, "pub_joint_trajectory_test");
11  ros::NodeHandle rosnode;
12  ros::Publisher pub_ = rosnode.advertise<trajectory_msgs::JointTrajectory>("set_joint_trajectory",100);
13  ros::ServiceClient srv_ = rosnode.serviceClient<gazebo_msgs::SetJointTrajectory>("set_joint_trajectory");
14 
15  trajectory_msgs::JointTrajectory jt;
16 
17  jt.header.stamp = ros::Time::now();
18  jt.header.frame_id = "pr2::torso_lift_link";
19 
20  jt.joint_names.push_back("r_shoulder_pan_joint");
21  jt.joint_names.push_back("r_shoulder_lift_joint");
22  jt.joint_names.push_back("r_upper_arm_roll_joint");
23  jt.joint_names.push_back("r_elbow_flex_joint");
24  jt.joint_names.push_back("r_forearm_roll_joint");
25  jt.joint_names.push_back("r_wrist_flex_joint");
26  jt.joint_names.push_back("l_shoulder_pan_joint");
27  jt.joint_names.push_back("l_shoulder_lift_joint");
28  jt.joint_names.push_back("l_upper_arm_roll_joint");
29  jt.joint_names.push_back("l_elbow_flex_joint");
30  jt.joint_names.push_back("l_forearm_roll_joint");
31  jt.joint_names.push_back("l_wrist_flex_joint");
32 
33  int n = 500;
34  double dt = 0.1;
35  double rps = 0.05;
36  jt.points.resize(n);
37  for (int i = 0; i < n; i++)
38  {
39  double theta = rps*2.0*M_PI*i*dt;
40  double x1 = -0.5*sin(2*theta);
41  double x2 = 0.5*sin(1*theta);
42  jt.points[i].positions.push_back(x1);
43  jt.points[i].positions.push_back(x2);
44  jt.points[i].positions.push_back(3.14);
45  jt.points[i].positions.push_back(-10.0);
46  jt.points[i].positions.push_back(-0.2);
47  jt.points[i].positions.push_back(-0.2);
48  jt.points[i].positions.push_back(x1);
49  jt.points[i].positions.push_back(x2);
50  jt.points[i].positions.push_back(3.14);
51  jt.points[i].positions.push_back(10.0);
52  jt.points[i].positions.push_back(-0.2);
53  jt.points[i].positions.push_back(-0.2);
54  // set duration
55  jt.points[i].time_from_start = ros::Duration(dt);
56  ROS_INFO_NAMED("joint_trajectory_test", "test: angles[%d][%f, %f]",n,x1,x2);
57  }
58 
59  // pub_.publish(jt); // use publisher
60 
61  gazebo_msgs::SetJointTrajectory sjt;
62  sjt.request.joint_trajectory = jt;
63  sjt.request.disable_physics_updates = false;
64 
65  ignition::math::Quaterniond r(0, 0, M_PI);
66  geometry_msgs::Pose p;
67  p.position.x = 0;
68  p.position.y = 0;
69  p.position.z = 0;
70  p.orientation.x = r.X();
71  p.orientation.y = r.Y();
72  p.orientation.z = r.Z();
73  p.orientation.w = r.W();
74  sjt.request.model_pose = p;
75  sjt.request.set_model_pose = true;
76 
77  srv_.call(sjt); // use service
78 
79  return 0;
80 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define ROS_INFO_NAMED(name,...)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static Time now()


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27