ros_client.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of JSK Lab, University of Tokyo. nor the
18  * names of its contributors may be used to endorse or promote products
19  * derived from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef ROS_CLIENT_HPP
36 #define ROS_CLIENT_HPP
37 
38 #include <ros/ros.h>
39 #include <control_msgs/FollowJointTrajectoryAction.h>
40 #include <control_msgs/FollowJointTrajectoryActionGoal.h>
41 #include <control_msgs/FollowJointTrajectoryGoal.h>
42 #include <trajectory_msgs/JointTrajectoryPoint.h>
44 #include <string>
45 #include <vector>
46 #include <cmath>
47 
49 {
50  /*
51  This class:
52 
53  - holds methods that are specific to Kawada Industries' dual-arm
54  robot called Hiro, via ROS.
55  - As of July 2014, this class is only intended to be used through HIRONX
56  class.
57  */
58 
59  // TODO(@iory): Address the following concern.
60  // This duplicates "Group" definition in HIRONX, which is bad.
61  // Need to consider consolidating the definition either in hironx_ros_bridge
62  // or somewhere in the upstream, e.g.:
63  // https://github.com/fkanehiro/hrpsys-base/pull/253
64 public:
66 
67  control_msgs::FollowJointTrajectoryGoal goal;
68 
70  GR_TORSO("torso"), GR_HEAD("head"), GR_LARM("larm"), GR_RARM("rarm"), MSG_ASK_ISSUEREPORT(
71  "Your report to https://github.com/start-jsk/rtmros_hironx/issues "
72  "about the issue you are seeing is appreciated.")
73  {
74  }
75 
76  explicit ROS_Client(std::vector<std::string> joingroups) :
78  "Your report to https://github.com/start-jsk/rtmros_hironx/issues "
79  "about the issue you are seeing is appreciated.")
80  {
81  set_groupnames(joingroups);
82  }
83 
84  ROS_Client(const ROS_Client& obj)
85  {
86  GR_TORSO = obj.GR_TORSO;
87  GR_HEAD = obj.GR_HEAD;
88  GR_LARM = obj.GR_LARM;
89  GR_RARM = obj.GR_RARM;
90  }
91 
93  {
94  GR_TORSO = obj.GR_TORSO;
95  GR_HEAD = obj.GR_HEAD;
96  GR_LARM = obj.GR_LARM;
97  GR_RARM = obj.GR_RARM;
98  return *this;
99  }
100 
102  {
103  }
104 
106  {
107  static TrajClient aclient_larm("/larm_controller/joint_trajectory_action", true);
108  static TrajClient aclient_rarm("/rarm_controller/joint_trajectory_action", true);
109  static TrajClient aclient_head("/head_controller/joint_trajectory_action", true);
110  static TrajClient aclient_torso("/torso_controller/joint_trajectory_action", true);
111 
112  aclient_larm.waitForServer();
113  ROS_INFO("ros_client; 1");
114  goal_larm = control_msgs::FollowJointTrajectoryGoal();
115  ROS_INFO("ros_client; 2");
116  goal_larm.trajectory.joint_names.push_back("LARM_JOINT0");
117  goal_larm.trajectory.joint_names.push_back("LARM_JOINT1");
118  goal_larm.trajectory.joint_names.push_back("LARM_JOINT2");
119  goal_larm.trajectory.joint_names.push_back("LARM_JOINT3");
120  goal_larm.trajectory.joint_names.push_back("LARM_JOINT4");
121  goal_larm.trajectory.joint_names.push_back("LARM_JOINT5");
122  ROS_INFO("ros_client; 3");
123 
124  aclient_rarm.waitForServer();
125  goal_rarm = control_msgs::FollowJointTrajectoryGoal();
126  goal_rarm.trajectory.joint_names.push_back("RARM_JOINT0");
127  goal_rarm.trajectory.joint_names.push_back("RARM_JOINT1");
128  goal_rarm.trajectory.joint_names.push_back("RARM_JOINT2");
129  goal_rarm.trajectory.joint_names.push_back("RARM_JOINT3");
130  goal_rarm.trajectory.joint_names.push_back("RARM_JOINT4");
131  goal_rarm.trajectory.joint_names.push_back("RARM_JOINT5");
132 
133  aclient_head.waitForServer();
134  goal_head = control_msgs::FollowJointTrajectoryGoal();
135  goal_head.trajectory.joint_names.push_back("HEAD_JOINT0");
136  goal_head.trajectory.joint_names.push_back("HEAD_JOINT1");
137 
138  aclient_torso.waitForServer();
139  goal_torso = control_msgs::FollowJointTrajectoryGoal();
140  goal_torso.trajectory.joint_names.push_back("CHEST_JOINT0");
141 
142  // Display Joint names
143  ROS_INFO("\nJoint names;");
144  std::cout << "Torso: [";
145  for (std::vector<std::string>::iterator name = goal_torso.trajectory.joint_names.begin();
146  name != goal_torso.trajectory.joint_names.end(); ++name)
147  std::cout << *name << " ";
148  std::cout << "]\nHead: [";
149  for (std::vector<std::string>::iterator name = goal_head.trajectory.joint_names.begin();
150  name != goal_head.trajectory.joint_names.end(); ++name)
151  std::cout << *name << " ";
152  std::cout << "]\nL-Arm: [";
153  for (std::vector<std::string>::iterator name = goal_larm.trajectory.joint_names.begin();
154  name != goal_larm.trajectory.joint_names.end(); ++name)
155  std::cout << *name << " ";
156  std::cout << "]\nR-Arm: [";
157  for (std::vector<std::string>::iterator name = goal_rarm.trajectory.joint_names.begin();
158  name != goal_rarm.trajectory.joint_names.end(); ++name)
159  std::cout << *name << " ";
160  std::cout << "]" << std::endl;
161  }
162 
163  // Init positions are taken from HIRONX.
164  // TODO(@iory): Need to add factory position too that's so convenient when
165  // working with the manufacturer.
166  void go_init(double task_duration = 7.0)
167  {
168  ROS_INFO("*** go_init begins ***");
169  std::vector<double> POSITIONS_TORSO_DEG(1, 0.0);
170  set_joint_angles_deg(GR_TORSO, POSITIONS_TORSO_DEG, task_duration);
171 
172  std::vector<double> POSITIONS_HEAD_DEG(2, 0.0);
173  set_joint_angles_deg(GR_HEAD, POSITIONS_HEAD_DEG, task_duration);
174 
175  std::vector<double> POSITIONS_LARM_DEG(6);
176  POSITIONS_LARM_DEG[0] = 0.6;
177  POSITIONS_LARM_DEG[1] = 0.0;
178  POSITIONS_LARM_DEG[2] = -100;
179  POSITIONS_LARM_DEG[3] = -15.2;
180  POSITIONS_LARM_DEG[4] = 9.4;
181  POSITIONS_LARM_DEG[5] = -3.2;
182  set_joint_angles_deg(GR_LARM, POSITIONS_LARM_DEG, task_duration);
183 
184  std::vector<double> POSITIONS_RARM_DEG(6);
185  POSITIONS_RARM_DEG[0] = -0.6;
186  POSITIONS_RARM_DEG[1] = 0;
187  POSITIONS_RARM_DEG[2] = -100;
188  POSITIONS_RARM_DEG[3] = 15.2;
189  POSITIONS_RARM_DEG[4] = 9.4;
190  POSITIONS_RARM_DEG[5] = 3.2;
191  set_joint_angles_deg(GR_RARM, POSITIONS_RARM_DEG, task_duration, true);
192 
193  ROS_INFO("*** go_init_ends ***");
194  }
195 
196  // This method takes the angles(radian) and
197  // changes the angle of the joints of the robot.
198  void set_joint_angles_rad(std::string groupname, std::vector<double> positions_radian, double duration = 7.0,
199  bool wait = false)
200  {
201  TrajClient* action_client;
202 
203  if (groupname == GR_TORSO)
204  {
205  action_client = new TrajClient("torso_controller/joint_trajectory_action", true);
206  goal = goal_torso;
207  }
208  else if (groupname == GR_HEAD)
209  {
210  action_client = new TrajClient("head_controller/joint_trajectory_action", true);
211  goal = goal_head;
212  }
213  else if (groupname == GR_LARM)
214  {
215  action_client = new TrajClient("larm_controller/joint_trajectory_action", true);
216  goal = goal_larm;
217  }
218  else if (groupname == GR_RARM)
219  {
220  action_client = new TrajClient("rarm_controller/joint_trajectory_action", true);
221  goal = goal_rarm;
222  }
223 
224  try
225  {
226  trajectory_msgs::JointTrajectoryPoint pt;
227  pt.positions = positions_radian;
228  pt.time_from_start = ros::Duration(duration);
229  goal.trajectory.points.clear();
230  goal.trajectory.points.push_back(pt);
231  goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(duration);
232  action_client->sendGoal(goal);
233  if (wait)
234  {
235  ROS_INFO("waiting......");
236  ROS_INFO("wait_for_result for the joint group %s = %d", groupname.c_str(), action_client->waitForResult());
237  }
238  }
239  catch (const ros::Exception& e)
240  {
241  ROS_INFO("%s", e.what());
242  }
243  catch (...)
244  {
245  ROS_INFO("Exception");
246  }
247  delete action_client;
248  }
249 
250  // groupname: This should exist in groupnames.
251  void set_joint_angles_deg(std::string groupname, std::vector<double> positions_deg, double duration = 7.0, bool wait =
252  false)
253  {
254  set_joint_angles_rad(groupname, to_rad_list(positions_deg), duration, wait);
255  }
256 
257 private:
258  std::string GR_TORSO;
259  std::string GR_HEAD;
260  std::string GR_LARM;
261  std::string GR_RARM;
262 
263  const std::string MSG_ASK_ISSUEREPORT;
264 
265  control_msgs::FollowJointTrajectoryGoal goal_larm;
266  control_msgs::FollowJointTrajectoryGoal goal_rarm;
267  control_msgs::FollowJointTrajectoryGoal goal_head;
268  control_msgs::FollowJointTrajectoryGoal goal_torso;
269 
270  /*
271  param groupnames: List of the joint group names. Assumes to be in the
272  following order:
273  torso, head, right arm, left arm.
274  This current setting is derived from the order of
275  Groups argument in HIRONX class. If other groups
276  need to be defined in the future, this method may
277  need to be modified.
278  */
279  void set_groupnames(std::vector<std::string> groupnames)
280  {
281  // output group name
282  ROS_INFO("set_groupnames");
283  for (std::vector<std::string>::iterator name = groupnames.begin(); name != groupnames.end(); ++name)
284  std::cout << *name << " ";
285  std::cout << std::endl;
286 
287  GR_TORSO = groupnames[0];
288  GR_HEAD = groupnames[1];
289  GR_RARM = groupnames[2];
290  GR_LARM = groupnames[3];
291  }
292 
293  // This method change degree to radian.
294  // param list_degree: A list length of the number of joints.
295  std::vector<double> to_rad_list(std::vector<double> list_degree)
296  {
297  std::vector<double> list_rad;
298  list_rad.reserve(list_degree.size());
299  for (std::vector<double>::iterator deg = list_degree.begin(); deg != list_degree.end(); ++deg)
300  {
301  double rad = *deg * M_PI / 180.0;
302  list_rad.push_back(rad);
303  ROS_INFO("Position deg=%lf rad=%lf", *deg, rad);
304  }
305  return list_rad;
306  }
307 };
308 
309 #endif
std::vector< double > to_rad_list(std::vector< double > list_degree)
Definition: ros_client.cpp:295
ROS_Client(const ROS_Client &obj)
Definition: ros_client.cpp:84
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient
Definition: ros_client.cpp:65
void go_init(double task_duration=7.0)
Definition: ros_client.cpp:166
void wait(int seconds)
control_msgs::FollowJointTrajectoryGoal goal_head
Definition: ros_client.cpp:267
name
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
std::string GR_TORSO
Definition: ros_client.cpp:258
#define ROS_INFO(...)
control_msgs::FollowJointTrajectoryGoal goal_rarm
Definition: ros_client.cpp:266
#define M_PI
std::string GR_LARM
Definition: ros_client.cpp:260
void set_joint_angles_rad(std::string groupname, std::vector< double > positions_radian, double duration=7.0, bool wait=false)
Definition: ros_client.cpp:198
ROS_Client & operator=(const ROS_Client &obj)
Definition: ros_client.cpp:92
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
void set_groupnames(std::vector< std::string > groupnames)
Definition: ros_client.cpp:279
control_msgs::FollowJointTrajectoryGoal goal_torso
Definition: ros_client.cpp:268
std::string GR_RARM
Definition: ros_client.cpp:261
void set_joint_angles_deg(std::string groupname, std::vector< double > positions_deg, double duration=7.0, bool wait=false)
Definition: ros_client.cpp:251
void init_action_clients()
Definition: ros_client.cpp:105
static Time now()
control_msgs::FollowJointTrajectoryGoal goal
Definition: ros_client.cpp:67
std::string GR_HEAD
Definition: ros_client.cpp:259
ROS_Client(std::vector< std::string > joingroups)
Definition: ros_client.cpp:76
control_msgs::FollowJointTrajectoryGoal goal_larm
Definition: ros_client.cpp:265
const std::string MSG_ASK_ISSUEREPORT
Definition: ros_client.cpp:263


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15