example_04_trajectory_node.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include "sensor_msgs/JointState.h"
4 #include "hebiros/AddGroupFromNamesSrv.h"
5 #include "hebiros/TrajectoryAction.h"
6 
7 #ifndef M_PI
8 #define M_PI 3.14159265358979323846
9 #endif
10 #ifndef M_PI_2
11 #define M_PI_2 1.57079632679489661923
12 #endif
13 
14 using namespace hebiros;
15 
16 
17 //Global variable and callback function used to store feedback data
18 sensor_msgs::JointState feedback;
19 
20 void feedback_callback(sensor_msgs::JointState data) {
21  feedback = data;
22 }
23 
24 //Callback which is called once when the action goal completes
26  const TrajectoryResultConstPtr& result) {
27  std::cout << "Final state: " << state.toString() << std::endl;
28 
29  for (int i = 0; i < result->final_state.name.size(); i++) {
30  std::cout << result->final_state.name[i] << ": " << std::endl;
31  std::cout << " Position: " << result->final_state.position[i] << std::endl;
32  std::cout << " Velocity: " << result->final_state.velocity[i] << std::endl;
33  std::cout << " Effort: " << result->final_state.effort[i] << std::endl;
34  }
35 
36  ros::shutdown();
37 }
38 
39 //Callback which is called once when the action goal becomes active
41 {
42  std:: cout << "Goal just went active" << std::endl;
43 }
44 
45 //Callback which is called every time feedback is received for the action goal
46 void trajectory_feedback(const TrajectoryFeedbackConstPtr& feedback)
47 {
48  std::cout << "Trajectory percent completion: " << feedback->percent_complete << std::endl;
49 }
50 
51 
52 int main(int argc, char **argv) {
53 
54  //Initialize ROS node
55  ros::init(argc, argv, "example_04_trajectory_node");
57  ros::Rate loop_rate(200);
58 
59  std::string group_name = "my_group";
60  int num_joints = 3;
61  int num_waypoints = 5;
62 
63  //Create a client which uses the service to create a group
64  ros::ServiceClient add_group_client = n.serviceClient<AddGroupFromNamesSrv>(
65  "/hebiros/add_group_from_names");
66 
67  //Create a subscriber to receive feedback from a group
68  //Register feedback_callback as a callback which runs when feedback is received
69  ros::Subscriber feedback_subscriber = n.subscribe(
70  "/hebiros/"+group_name+"/feedback/joint_state", 100, feedback_callback);
71 
72  AddGroupFromNamesSrv add_group_srv;
73 
74  //Construct a group using 3 known modules
75  add_group_srv.request.group_name = group_name;
76  add_group_srv.request.names = {"base", "shoulder", "elbow"};
77  add_group_srv.request.families = {"HEBI"};
78  //Call the add_group_from_urdf service to create a group until it succeeds
79  //Specific topics and services will now be available under this group's namespace
80  while(!add_group_client.call(add_group_srv)) {}
81 
82  feedback.position.reserve(3);
83 
84  //Create an action client for executing a trajectory
85  actionlib::SimpleActionClient<TrajectoryAction> client("/hebiros/"+group_name+"/trajectory", true);
86  //Wait for the action server corresponding to the action client
87  client.waitForServer();
88 
89  //Construct a trajectory to be sent as an action goal
90  TrajectoryGoal goal;
91  goal.times.resize(num_waypoints);
92  goal.waypoints.resize(num_waypoints);
93 
94  WaypointMsg waypoint;
95  waypoint.names.resize(num_joints);
96  waypoint.positions.resize(num_joints);
97  waypoint.velocities.resize(num_joints);
98  waypoint.accelerations.resize(num_joints);
99 
100  double nan = std::numeric_limits<float>::quiet_NaN();
101  //Set the times to reach each waypoint in seconds
102  std::vector<double> times = {0, 5, 10, 15, 20};
103  std::vector<std::string> names = {"HEBI/base", "HEBI/shoulder", "HEBI/elbow"};
104  //Set positions, velocities, and accelerations for each waypoint and each joint
105  //The following vectors have one joint per row and one waypoint per column
106  std::vector<std::vector<double>> positions = {{feedback.position[0], 0, M_PI_2, 0, 0},
107  {feedback.position[1], 0, M_PI_2, M_PI_2, 0},
108  {feedback.position[2], 0, 0, M_PI_2, 0}};
109  std::vector<std::vector<double>> velocities = {{0, nan, nan, nan, 0},
110  {0, nan, nan, nan, 0},
111  {0, nan, nan, nan, 0}};
112  std::vector<std::vector<double>> accelerations = {{0, nan, nan, nan, 0},
113  {0, nan, nan, nan, 0},
114  {0, nan, nan, nan, 0}};
115 
116  //Construct the goal using the TrajectoryGoal format
117  for (int i = 0; i < num_waypoints; i++) {
118  for (int j = 0; j < num_joints; j++) {
119  waypoint.names[j] = names[j];
120  waypoint.positions[j] = positions[j][i];
121  waypoint.velocities[j] = velocities[j][i];
122  waypoint.accelerations[j] = accelerations[j][i];
123  }
124  goal.times[i] = times[i];
125  goal.waypoints[i] = waypoint;
126  }
127 
128  //Send the goal, executing the trajectory
130 
131  while(ros::ok()) {
132  ros::spinOnce();
133  loop_rate.sleep();
134  }
135 
136  return 0;
137 }
138 
139 
140 
141 
142 
143 
144 
145 
146 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void feedback_callback(sensor_msgs::JointState data)
void trajectory_active()
#define M_PI_2
sensor_msgs::JointState feedback
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
void trajectory_feedback(const TrajectoryFeedbackConstPtr &feedback)
std::string toString() const
ROSCPP_DECL bool ok()
int main(int argc, char **argv)
bool sleep()
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
ROSCPP_DECL void shutdown()
void trajectory_done(const actionlib::SimpleClientGoalState &state, const TrajectoryResultConstPtr &result)
ROSCPP_DECL void spinOnce()


hebiros_basic_examples
Author(s): Xavier Artache , Matthew Tesch
autogenerated on Thu Sep 3 2020 04:13:38