ros_client.py
Go to the documentation of this file.
1 
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2014, Tokyo Opensource Robotics Kyokai Association (TORK)
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
19 # names of its contributors may be used to endorse or promote products
20 # derived from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 
35 import copy
36 import math
37 import socket
38 
39 import actionlib
40 import moveit_commander
41 from moveit_commander import MoveGroupCommander, MoveItCommanderException, RobotCommander
42 import rospy
43 from rospy import ROSInitException
44 from control_msgs.msg import FollowJointTrajectoryAction
45 from control_msgs.msg import FollowJointTrajectoryGoal
46 from geometry_msgs.msg import Pose
47 from trajectory_msgs.msg import JointTrajectoryPoint
48 from tf.transformations import quaternion_from_euler, euler_from_quaternion, compose_matrix, translation_from_matrix, euler_from_matrix
49 import numpy
50 
51 from hironx_ros_bridge.constant import Constant
52 
53 _MSG_ASK_ISSUEREPORT = 'Your report to ' + \
54  'https://github.com/start-jsk/rtmros_hironx/issues ' + \
55  'about the issue you are seeing is appreciated.'
56 
57 
58 # workaround for core dump whenever exiting Python MoveIt script (https://github.com/ros-planning/moveit_commander/issues/15#issuecomment-34441531)
59 import atexit, os
60 atexit.register(lambda : os._exit(0))
61 
62 class ROS_Client(RobotCommander):
63  '''
64  This class holds methods that are specific to Kawada Industries' dual-arm
65  robot called Hiro, via ROS.
66 
67  @since: December 2014: Now this class is replacing the default programming interface
68  with HIRONX (hironx_client.py).
69  @since: July 2014: this class is only intended to be used through HIRONX
70  class.
71  '''
72 
73  _MSG_NO_MOVEGROUP_FOUND = ('\nMake sure you\'ve launched MoveGroup ' +
74  '(e.g. by launching ' +
75  'moveit_planning_execution.launch)')
76 
77  def __init__(self, jointgroups=None):
78  '''
79  @param jointgroups [str]: Deprecated. No need after version 1.1.4 onward.
80  '''
81  # if we do not have ros running, return
82  try:
83  rospy.get_master().getSystemState()
84  except socket.error as e:
85  errormsg = 'No ROS Master found. Without it, you cannot use ROS from this script, but you can still use RTM without issues. ' + \
86  'To use ROS, do not forget to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN'
87  raise ROSInitException(errormsg)
88  except Exception as e:
89  errormsg = '[ros_client] Unknown exception occurred, so do not create ros client...'
90  rospy.logerr(errormsg)
91  raise e
92 
93  if not rospy.has_param('robot_description'):
94  rospy.logwarn('ROS Bridge is not started yet, Assuming you want just to use RTM')
95  return
96 
97  super(ROS_Client, self).__init__() # This solves https://github.com/start-jsk/rtmros_hironx/issues/300
98 
99  rospy.init_node('hironx_ros_client')
100 
101  # See the doc in the method for the reason why this line is still kept
102  # even after this class has shifted MoveIt! intensive.
103  self._init_action_clients()
104 
105  if not rospy.has_param('robot_description_semantic'):
106  rospy.logwarn('Moveit is not started yet, if you want to use MoveIt!' + self._MSG_NO_MOVEGROUP_FOUND)
107  return
108 
109  try:
111  except RuntimeError as e:
112  rospy.logerr(str(e) + self._MSG_NO_MOVEGROUP_FOUND)
113 
114 # def __getattr__(self, name):
115 # '''
116 # Trying to resolve https://github.com/start-jsk/rtmros_hironx/issues/300
117 # '''
118 # return object.__getattr__(self, name)
119 
121  '''
122  @raise RuntimeError: When MoveGroup is not running.
123  '''
124  # left_arm, right_arm are fixed in nextage_moveit_config pkg.
125  try:
126  self.MG_LARM = self.get_group(Constant.GRNAME_LEFT_ARM_MOVEGROUP)
127  self.MG_RARM = self.get_group(Constant.GRNAME_RIGHT_ARM_MOVEGROUP)
128  self.MG_BOTHARMS = self.get_group(Constant.GRNAME_BOTH_ARMS)
129  self.MG_HEAD = self.get_group(Constant.GRNAME_HEAD)
130  self.MG_TORSO = self.get_group(Constant.GRNAME_TORSO)
131  self.MG_UPPERBODY = self.get_group(Constant.GRNAME_UPPERBODY)
132  self.MG_LARM.set_planner_id("RRTConnectkConfigDefault")
133  self.MG_RARM.set_planner_id("RRTConnectkConfigDefault")
134  self.MG_BOTHARMS.set_planner_id("RRTConnectkConfigDefault")
135  self.MG_HEAD.set_planner_id("RRTConnectkConfigDefault")
136  self.MG_TORSO.set_planner_id("RRTConnectkConfigDefault")
137  self.MG_UPPERBODY.set_planner_id("RRTConnectkConfigDefault")
138 
139  # TODO: Why the ref frames need to be kept as member variables?
140  self._movegr_larm_ref_frame = self.MG_LARM.get_pose_reference_frame()
141  self._movegr_rarm_ref_frame = self.MG_RARM.get_pose_reference_frame()
142  self._movegr_botharms_ref_frame = self.MG_BOTHARMS.get_pose_reference_frame()
143  except RuntimeError as e:
144  raise e
145 
147  '''
148  This is only needed for accessing Actionlib clients directly, which
149  is no longer needed for this class now that it inherits
150  RobotCommander from MoveIt!. Still this line is kept for the methods
151  deprecated but remain for backward compatibility.
152  '''
154  '/larm_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
156  '/rarm_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
158  '/head_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
160  '/torso_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
161 
162  self._aclient_larm.wait_for_server()
163  self._goal_larm = FollowJointTrajectoryGoal()
164  self._goal_larm.trajectory.joint_names.append("LARM_JOINT0")
165  self._goal_larm.trajectory.joint_names.append("LARM_JOINT1")
166  self._goal_larm.trajectory.joint_names.append("LARM_JOINT2")
167  self._goal_larm.trajectory.joint_names.append("LARM_JOINT3")
168  self._goal_larm.trajectory.joint_names.append("LARM_JOINT4")
169  self._goal_larm.trajectory.joint_names.append("LARM_JOINT5")
170 
171  self._aclient_rarm.wait_for_server()
172  self._goal_rarm = FollowJointTrajectoryGoal()
173  self._goal_rarm.trajectory.joint_names.append("RARM_JOINT0")
174  self._goal_rarm.trajectory.joint_names.append("RARM_JOINT1")
175  self._goal_rarm.trajectory.joint_names.append("RARM_JOINT2")
176  self._goal_rarm.trajectory.joint_names.append("RARM_JOINT3")
177  self._goal_rarm.trajectory.joint_names.append("RARM_JOINT4")
178  self._goal_rarm.trajectory.joint_names.append("RARM_JOINT5")
179 
180  self._aclient_head.wait_for_server()
181  self._goal_head = FollowJointTrajectoryGoal()
182  self._goal_head.trajectory.joint_names.append('HEAD_JOINT0')
183  self._goal_head.trajectory.joint_names.append('HEAD_JOINT1')
184 
185  self._aclient_torso.wait_for_server()
186  self._goal_torso = FollowJointTrajectoryGoal()
187  self._goal_torso.trajectory.joint_names.append('CHEST_JOINT0')
188 
189  rospy.loginfo('Joint names; ' +
190  'Torso: {}\n\tHead: {}\n\tL-Arm: {}\n\tR-Arm: {}'.format(
191  self._goal_torso.trajectory.joint_names,
192  self._goal_head.trajectory.joint_names,
193  self._goal_larm.trajectory.joint_names,
194  self._goal_rarm.trajectory.joint_names))
195 
196  def go_init(self, init_pose_type=0, task_duration=7.0):
197  '''
198  Change the posture of the entire robot to the pre-defined init pose.
199 
200  This method is equivalent to
201  hironx_ros_bridge.hironx_client.HIRONX.goInitial method (https://github.com/start-jsk/rtmros_hironx/blob/83c3ff0ad2aabd8525631b08276d33b09c98b2bf/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L199),
202  with an addition of utilizing MoveIt!, which means e.g. if there is
203  possible perceived collision robots will take the path MoveIt! computes
204  with collision avoidance taken into account.
205 
206  @type task_duration: float
207  @param init_pose_type:
208  0: default init pose (specified as _InitialPose)
209  1: factory init pose (specified as _InitialPose_Factory)
210  '''
211  rospy.loginfo('*** go_init begins ***')
212  posetype_str = ''
213  if 0 == init_pose_type:
214  posetype_str = 'init_rtm'
215  elif 1 == init_pose_type:
216  posetype_str = 'init_rtm_factory'
217  else:
218  rospy.logerr("unsupporeted init_pose_type " + str(init_pose_type))
219  self.MG_BOTHARMS.set_named_target(posetype_str)
220  self.MG_BOTHARMS.go()
221 
222  def go_offpose(self, task_duration=7.0):
223  self.MG_UPPERBODY.set_named_target(Constant.POSE_OFF)
224  self.MG_UPPERBODY.go()
225 
226  def goInitial(self, init_pose_type=0, task_duration=7.0):
227  '''
228  This method internally calls self.go_init.
229 
230  This method exists solely because of compatibility purpose with
231  hironx_ros_bridge.hironx_client.HIRONX.goInitial, which
232  holds a method "goInitial".
233 
234  @param init_pose_type:
235  0: default init pose (specified as _InitialPose)
236  1: factory init pose (specified as _InitialPose_Factory)
237  '''
238  return self.go_init(init_pose_type, task_duration)
239 
240  def set_joint_angles_rad(self, groupname, positions_radian, duration=7.0,
241  wait=False):
242  '''
243  @deprecated: Use MoveitCommander.set_joint_value_target instead.
244 
245  @type groupname: str
246  @param groupname: This should exist in self.groupnames.
247  @type positions_radian: [float]
248  @type duration: float
249  @type wait: bool
250  '''
251  if groupname == Constant.GRNAME_TORSO:
252  action_client = self._aclient_torso
253  goal = self._goal_torso
254  elif groupname == Constant.GRNAME_HEAD:
255  action_client = self._aclient_head
256  goal = self._goal_head
257  elif groupname == Constant.GRNAME_LEFT_ARM:
258  action_client = self._aclient_larm
259  goal = self._goal_larm
260  elif groupname == Constant.GRNAME_RIGHT_ARM:
261  action_client = self._aclient_rarm
262  goal = self._goal_rarm
263  else:
264  # TODO: Throw exception; a valid group name isn't passed.
265  rospy.logerr('groupname {} not assigned'.format(groupname))
266 
267  try:
268  pt = JointTrajectoryPoint()
269  pt.positions = positions_radian
270  pt.time_from_start = rospy.Duration(duration)
271  goal.trajectory.points = [pt]
272  goal.trajectory.header.stamp = \
273  rospy.Time.now() + rospy.Duration(duration)
274 
275  action_client.send_goal(goal)
276  if wait:
277  rospy.loginfo(
278  'wait_for_result for the joint group {} = {}'.format(
279  groupname, action_client.wait_for_result()))
280  except rospy.ROSInterruptException as e:
281  rospy.loginfo(e.str())
282 
283  def set_joint_angles_deg(self, groupname, positions_deg, duration=7.0,
284  wait=False):
285  '''
286  @type groupname: str
287  @param groupname: This should exist in self.groupnames.
288  @type positions_deg: [float]
289  @type duration: float
290  @type wait: bool
291  '''
292  self.set_joint_angles_rad(groupname, self._to_rad_list(positions_deg),
293  duration, wait)
294 
295  def _to_rad_list(self, list_degree):
296  '''
297  @TODO Needs to be replaced by something more common, or at least moved
298  somewhere more common.
299 
300  @type list_degree: [float]
301  @param list_degree: A list length of the number of joints.
302  '''
303  list_rad = []
304  for deg in list_degree:
305  rad = math.radians(deg)
306  list_rad.append(rad)
307  rospy.logdebug('Position deg={} rad={}'.format(deg, rad))
308  return list_rad
309 
310  def set_pose(self, joint_group, position, rpy=None, task_duration=7.0,
311  do_wait=True, ref_frame_name=None):
312  '''
313  @deprecated: Use set_pose_target (from MoveGroupCommander) directly.
314  Accept pose defined by position and RPY in Cartesian format.
315 
316  @type joint_group: str
317  @type position: [float]
318  @param position: x, y, z.
319  @type rpy: [float]
320  @param rpy: If None, keep the current orientation by using
321  MoveGroupCommander.set_position_target. See:
322  'http://moveit.ros.org/doxygen/' +
323  'classmoveit__commander_1_1move__group_1_1' +
324  'MoveGroupCommander.html#acfe2220fd85eeb0a971c51353e437753'
325  @param ref_frame_name: reference frame for target pose, i.e. "LARM_JOINT5_Link".
326  '''
327  # convert to tuple to list
328  position = list(position)
329  if not rpy is None:
330  rpy = list(rpy)
331  #
332  # Check if MoveGroup is instantiated.
333  if not self.MG_LARM or not self.MG_RARM:
334  try:
336  except RuntimeError as e:
337  rospy.logerr(self._MSG_NO_MOVEGROUP_FOUND)
338  raise e
339 
340  # Locally assign the specified MoveGroup
341  movegr = None
342  if Constant.GRNAME_LEFT_ARM == joint_group:
343  movegr = self.MG_LARM
344  elif Constant.GRNAME_RIGHT_ARM == joint_group:
345  movegr = self.MG_RARM
346  else:
347  rospy.logerr('joint_group must be either %s, %s or %s'%(Constant.GRNAME_LEFT_ARM,
348  Constant.GRNAME_RIGHT_ARM,
349  Constant.GRNAME_BOTH_ARMS))
350  return
351 
352  # set reference frame
353  if ref_frame_name :
354  ref_pose = movegr.get_current_pose(ref_frame_name).pose
355  ref_mat = compose_matrix(
356  translate = [ref_pose.position.x, ref_pose.position.y, ref_pose.position.z],
357  angles = list(euler_from_quaternion([ref_pose.orientation.x,ref_pose.orientation.y,ref_pose.orientation.z,ref_pose.orientation.w])))
358  target_mat = numpy.dot(ref_mat, compose_matrix(translate = position, angles = rpy or [0, 0, 0]))
359  position = list(translation_from_matrix(target_mat))
360  rpy = list(euler_from_matrix(target_mat))
361 
362  # If no RPY specified, give position and return the method.
363  if not rpy:
364  try:
365  movegr.set_position_target(position)
366  except MoveItCommanderException as e:
367  rospy.logerr(str(e))
368  (movegr.go(do_wait) or movegr.go(do_wait) or
369  rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))
370  return
371 
372  # Not necessary to convert from rpy to quaternion, since
373  # MoveGroupCommander.set_pose_target can take rpy format too.
374  pose = copy.deepcopy(position)
375  pose.extend(rpy)
376  rospy.loginfo('setpose jntgr={} mvgr={} pose={} posi={} rpy={}'.format(
377  joint_group, movegr, pose, position, rpy))
378  try:
379  movegr.set_pose_target(pose)
380  except MoveItCommanderException as e:
381  rospy.logerr(str(e))
382  except Exception as e:
383  rospy.logerr(str(e))
384 
385  (movegr.go(do_wait) or movegr.go(do_wait) or
386  rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))
def set_pose(self, joint_group, position, rpy=None, task_duration=7.0, do_wait=True, ref_frame_name=None)
Definition: ros_client.py:311
def _to_rad_list(self, list_degree)
Definition: ros_client.py:295
def go_offpose(self, task_duration=7.0)
Definition: ros_client.py:222
def set_joint_angles_rad(self, groupname, positions_radian, duration=7.0, wait=False)
Definition: ros_client.py:241
def goInitial(self, init_pose_type=0, task_duration=7.0)
Definition: ros_client.py:226
def set_joint_angles_deg(self, groupname, positions_deg, duration=7.0, wait=False)
Definition: ros_client.py:284
def __init__(self, jointgroups=None)
Definition: ros_client.py:77
def go_init(self, init_pose_type=0, task_duration=7.0)
Definition: ros_client.py:196


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