ros_client.py
Go to the documentation of this file.
00001 ## -*- coding: utf-8 -*-
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2014, Tokyo Opensource Robotics Kyokai Association (TORK)
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
00019 #    names of its contributors may be used to endorse or promote products
00020 #    derived from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 import copy
00036 import math
00037 import socket
00038 
00039 import actionlib
00040 import moveit_commander
00041 from moveit_commander import MoveGroupCommander, MoveItCommanderException, RobotCommander
00042 import rospy
00043 from rospy import ROSInitException
00044 from control_msgs.msg import FollowJointTrajectoryAction
00045 from control_msgs.msg import FollowJointTrajectoryGoal
00046 from geometry_msgs.msg import Pose
00047 from trajectory_msgs.msg import JointTrajectoryPoint
00048 from tf.transformations import quaternion_from_euler, euler_from_quaternion, compose_matrix, translation_from_matrix, euler_from_matrix
00049 import numpy
00050 
00051 from hironx_ros_bridge.constant import Constant
00052 
00053 _MSG_ASK_ISSUEREPORT = 'Your report to ' + \
00054                        'https://github.com/start-jsk/rtmros_hironx/issues ' + \
00055                        'about the issue you are seeing is appreciated.'
00056 
00057 
00058 # workaround for core dump whenever exiting Python MoveIt script (https://github.com/ros-planning/moveit_commander/issues/15#issuecomment-34441531)
00059 import atexit, os
00060 atexit.register(lambda : os._exit(0))
00061 
00062 class ROS_Client(RobotCommander):
00063     '''
00064     This class holds methods that are specific to Kawada Industries' dual-arm
00065     robot called Hiro, via ROS.
00066 
00067     @since: December 2014: Now this class is replacing the default programming interface
00068             with HIRONX (hironx_client.py).
00069     @since: July 2014: this class is only intended to be used through HIRONX
00070       class.
00071     '''
00072 
00073     _MSG_NO_MOVEGROUP_FOUND = ('\nMake sure you\'ve launched MoveGroup ' +
00074                                '(e.g. by launching ' +
00075                                'moveit_planning_execution.launch)')
00076 
00077     def __init__(self, jointgroups=None):
00078         '''
00079         @param jointgroups [str]: Deprecated. No need after version 1.1.4 onward.
00080         '''
00081         # if we do not have ros running, return 
00082         try:
00083             rospy.get_master().getSystemState()
00084         except socket.error as e:
00085             errormsg = 'No ROS Master found. Without it, you cannot use ROS from this script, but you can still use RTM without issues. ' + \
00086                        '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'
00087             raise ROSInitException(errormsg)
00088         except Exception as e:
00089             errormsg = '[ros_client] Unknown exception occurred, so do not create ros client...'
00090             rospy.logerr(errormsg)
00091             raise e
00092 
00093         if not rospy.has_param('robot_description'):
00094             rospy.logwarn('ROS Bridge is not started yet, Assuming you want just to use RTM')
00095             return
00096 
00097         super(ROS_Client, self).__init__()  # This solves https://github.com/start-jsk/rtmros_hironx/issues/300
00098 
00099         rospy.init_node('hironx_ros_client')
00100 
00101         # See the doc in the method for the reason why this line is still kept
00102         # even after this class has shifted MoveIt! intensive.
00103         self._init_action_clients()
00104 
00105         if not rospy.has_param('robot_description_semantic'):
00106             rospy.logwarn('Moveit is not started yet, if you want to use MoveIt!' + self._MSG_NO_MOVEGROUP_FOUND)
00107             return
00108 
00109         try:
00110             self._init_moveit_commanders()
00111         except RuntimeError as e:
00112             rospy.logerr(str(e) + self._MSG_NO_MOVEGROUP_FOUND)
00113 
00114 #    def __getattr__(self, name):
00115 #        '''
00116 #        Trying to resolve https://github.com/start-jsk/rtmros_hironx/issues/300
00117 #        '''
00118 #        return object.__getattr__(self, name)
00119 
00120     def _init_moveit_commanders(self):
00121         '''
00122         @raise RuntimeError: When MoveGroup is not running.
00123         '''
00124         # left_arm, right_arm are fixed in nextage_moveit_config pkg.
00125         try:
00126             self.MG_LARM = self.get_group(Constant.GRNAME_LEFT_ARM_MOVEGROUP)
00127             self.MG_RARM = self.get_group(Constant.GRNAME_RIGHT_ARM_MOVEGROUP)
00128             self.MG_BOTHARMS = self.get_group(Constant.GRNAME_BOTH_ARMS)
00129             self.MG_HEAD = self.get_group(Constant.GRNAME_HEAD)
00130             self.MG_TORSO = self.get_group(Constant.GRNAME_TORSO)
00131             self.MG_UPPERBODY = self.get_group(Constant.GRNAME_UPPERBODY)
00132             self.MG_LARM.set_planner_id("RRTConnectkConfigDefault")
00133             self.MG_RARM.set_planner_id("RRTConnectkConfigDefault")
00134             self.MG_BOTHARMS.set_planner_id("RRTConnectkConfigDefault")
00135             self.MG_HEAD.set_planner_id("RRTConnectkConfigDefault")
00136             self.MG_TORSO.set_planner_id("RRTConnectkConfigDefault")
00137             self.MG_UPPERBODY.set_planner_id("RRTConnectkConfigDefault")
00138 
00139             # TODO: Why the ref frames need to be kept as member variables?
00140             self._movegr_larm_ref_frame = self.MG_LARM.get_pose_reference_frame()
00141             self._movegr_rarm_ref_frame = self.MG_RARM.get_pose_reference_frame()
00142             self._movegr_botharms_ref_frame = self.MG_BOTHARMS.get_pose_reference_frame()
00143         except RuntimeError as e:
00144             raise e
00145 
00146     def _init_action_clients(self):
00147         '''
00148         This is only needed for accessing Actionlib clients directly, which
00149         is no longer needed for this class now that it inherits
00150         RobotCommander from MoveIt!. Still this line is kept for the methods
00151         deprecated but remain for backward compatibility.
00152         '''
00153         self._aclient_larm = actionlib.SimpleActionClient(
00154             '/larm_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00155         self._aclient_rarm = actionlib.SimpleActionClient(
00156             '/rarm_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00157         self._aclient_head = actionlib.SimpleActionClient(
00158             '/head_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00159         self._aclient_torso = actionlib.SimpleActionClient(
00160             '/torso_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction)
00161 
00162         self._aclient_larm.wait_for_server()
00163         self._goal_larm = FollowJointTrajectoryGoal()
00164         self._goal_larm.trajectory.joint_names.append("LARM_JOINT0")
00165         self._goal_larm.trajectory.joint_names.append("LARM_JOINT1")
00166         self._goal_larm.trajectory.joint_names.append("LARM_JOINT2")
00167         self._goal_larm.trajectory.joint_names.append("LARM_JOINT3")
00168         self._goal_larm.trajectory.joint_names.append("LARM_JOINT4")
00169         self._goal_larm.trajectory.joint_names.append("LARM_JOINT5")
00170 
00171         self._aclient_rarm.wait_for_server()
00172         self._goal_rarm = FollowJointTrajectoryGoal()
00173         self._goal_rarm.trajectory.joint_names.append("RARM_JOINT0")
00174         self._goal_rarm.trajectory.joint_names.append("RARM_JOINT1")
00175         self._goal_rarm.trajectory.joint_names.append("RARM_JOINT2")
00176         self._goal_rarm.trajectory.joint_names.append("RARM_JOINT3")
00177         self._goal_rarm.trajectory.joint_names.append("RARM_JOINT4")
00178         self._goal_rarm.trajectory.joint_names.append("RARM_JOINT5")
00179 
00180         self._aclient_head.wait_for_server()
00181         self._goal_head = FollowJointTrajectoryGoal()
00182         self._goal_head.trajectory.joint_names.append('HEAD_JOINT0')
00183         self._goal_head.trajectory.joint_names.append('HEAD_JOINT1')
00184 
00185         self._aclient_torso.wait_for_server()
00186         self._goal_torso = FollowJointTrajectoryGoal()
00187         self._goal_torso.trajectory.joint_names.append('CHEST_JOINT0')
00188 
00189         rospy.loginfo('Joint names; ' +
00190                       'Torso: {}\n\tHead: {}\n\tL-Arm: {}\n\tR-Arm: {}'.format(
00191                           self._goal_torso.trajectory.joint_names,
00192                           self._goal_head.trajectory.joint_names,
00193                           self._goal_larm.trajectory.joint_names,
00194                           self._goal_rarm.trajectory.joint_names))
00195 
00196     def go_init(self, init_pose_type=0, task_duration=7.0):
00197         '''
00198         Change the posture of the entire robot to the pre-defined init pose.
00199 
00200         This method is equivalent to
00201         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),
00202         with an addition of utilizing MoveIt!, which means e.g. if there is
00203         possible perceived collision robots will take the path MoveIt! computes
00204         with collision avoidance taken into account.
00205 
00206         @type task_duration: float
00207         @param init_pose_type:
00208                0: default init pose (specified as _InitialPose)
00209                1: factory init pose (specified as _InitialPose_Factory)
00210         '''
00211         rospy.loginfo('*** go_init begins ***')
00212         posetype_str = ''
00213         if 0 == init_pose_type:
00214             posetype_str = 'init_rtm'
00215         elif 1 == init_pose_type:
00216             posetype_str = 'init_rtm_factory'
00217         else:
00218             rospy.logerr("unsupporeted init_pose_type " + str(init_pose_type))
00219         self.MG_BOTHARMS.set_named_target(posetype_str)
00220         self.MG_BOTHARMS.go()
00221 
00222     def go_offpose(self, task_duration=7.0):
00223         self.MG_UPPERBODY.set_named_target(Constant.POSE_OFF)
00224         self.MG_UPPERBODY.go()
00225 
00226     def goInitial(self, init_pose_type=0, task_duration=7.0):
00227         '''
00228         This method internally calls self.go_init.
00229 
00230         This method exists solely because of compatibility purpose with
00231         hironx_ros_bridge.hironx_client.HIRONX.goInitial, which
00232         holds a method "goInitial".
00233 
00234         @param init_pose_type:
00235                0: default init pose (specified as _InitialPose)
00236                1: factory init pose (specified as _InitialPose_Factory)
00237         '''
00238         return self.go_init(init_pose_type, task_duration)
00239 
00240     def set_joint_angles_rad(self, groupname, positions_radian, duration=7.0,
00241                              wait=False):
00242         '''
00243         @deprecated: Use MoveitCommander.set_joint_value_target instead.
00244 
00245         @type groupname: str
00246         @param groupname: This should exist in self.groupnames.
00247         @type positions_radian: [float]
00248         @type duration: float
00249         @type wait: bool
00250         '''
00251         if groupname == Constant.GRNAME_TORSO:
00252             action_client = self._aclient_torso
00253             goal = self._goal_torso
00254         elif groupname == Constant.GRNAME_HEAD:
00255             action_client = self._aclient_head
00256             goal = self._goal_head
00257         elif groupname == Constant.GRNAME_LEFT_ARM:
00258             action_client = self._aclient_larm
00259             goal = self._goal_larm
00260         elif groupname == Constant.GRNAME_RIGHT_ARM:
00261             action_client = self._aclient_rarm
00262             goal = self._goal_rarm
00263         else:
00264             # TODO: Throw exception; a valid group name isn't passed.
00265             rospy.logerr('groupname {} not assigned'.format(groupname))
00266 
00267         try:
00268             pt = JointTrajectoryPoint()
00269             pt.positions = positions_radian
00270             pt.time_from_start = rospy.Duration(duration)
00271             goal.trajectory.points = [pt]
00272             goal.trajectory.header.stamp = \
00273                 rospy.Time.now() + rospy.Duration(duration)
00274 
00275             action_client.send_goal(goal)
00276             if wait:
00277                 rospy.loginfo(
00278                     'wait_for_result for the joint group {} = {}'.format(
00279                         groupname, action_client.wait_for_result()))
00280         except rospy.ROSInterruptException as e:
00281             rospy.loginfo(e.str())
00282 
00283     def set_joint_angles_deg(self, groupname, positions_deg, duration=7.0,
00284                              wait=False):
00285         '''
00286         @type groupname: str
00287         @param groupname: This should exist in self.groupnames.
00288         @type positions_deg: [float]
00289         @type duration: float
00290         @type wait: bool
00291         '''
00292         self.set_joint_angles_rad(groupname, self._to_rad_list(positions_deg),
00293                                   duration, wait)
00294 
00295     def _to_rad_list(self, list_degree):
00296         '''
00297         @TODO Needs to be replaced by something more common, or at least moved
00298               somewhere more common.
00299 
00300         @type list_degree: [float]
00301         @param list_degree: A list length of the number of joints.
00302         '''
00303         list_rad = []
00304         for deg in list_degree:
00305             rad = math.radians(deg)
00306             list_rad.append(rad)
00307             rospy.logdebug('Position deg={} rad={}'.format(deg, rad))
00308         return list_rad
00309 
00310     def set_pose(self, joint_group, position, rpy=None, task_duration=7.0,
00311                  do_wait=True, ref_frame_name=None):
00312         '''
00313         @deprecated: Use set_pose_target (from MoveGroupCommander) directly.
00314         Accept pose defined by position and RPY in Cartesian format.
00315 
00316         @type joint_group: str
00317         @type position: [float]
00318         @param position: x, y, z.
00319         @type rpy: [float]
00320         @param rpy: If None, keep the current orientation by using
00321                     MoveGroupCommander.set_position_target. See:
00322                      'http://moveit.ros.org/doxygen/' +
00323                      'classmoveit__commander_1_1move__group_1_1' +
00324                      'MoveGroupCommander.html#acfe2220fd85eeb0a971c51353e437753'
00325         @param ref_frame_name: reference frame for target pose, i.e. "LARM_JOINT5_Link".
00326         '''
00327         # convert to tuple to list
00328         position = list(position)
00329         if not rpy is None:
00330             rpy = list(rpy)
00331         #
00332         # Check if MoveGroup is instantiated.
00333         if not self.MG_LARM or not self.MG_RARM:
00334             try:
00335                 self._init_moveit_commanders()
00336             except RuntimeError as e:
00337                 rospy.logerr(self._MSG_NO_MOVEGROUP_FOUND)
00338                 raise e
00339 
00340         # Locally assign the specified MoveGroup
00341         movegr = None
00342         if Constant.GRNAME_LEFT_ARM == joint_group:
00343             movegr = self.MG_LARM
00344         elif Constant.GRNAME_RIGHT_ARM == joint_group:
00345             movegr = self.MG_RARM
00346         else:
00347             rospy.logerr('joint_group must be either %s, %s or %s'%(Constant.GRNAME_LEFT_ARM,
00348                                                                     Constant.GRNAME_RIGHT_ARM,
00349                                                                     Constant.GRNAME_BOTH_ARMS))
00350             return
00351 
00352         # set reference frame
00353         if ref_frame_name :
00354             ref_pose = movegr.get_current_pose(ref_frame_name).pose
00355             ref_mat = compose_matrix(
00356                 translate = [ref_pose.position.x, ref_pose.position.y, ref_pose.position.z],
00357                 angles = list(euler_from_quaternion([ref_pose.orientation.x,ref_pose.orientation.y,ref_pose.orientation.z,ref_pose.orientation.w])))
00358             target_mat = numpy.dot(ref_mat, compose_matrix(translate = position, angles = rpy or [0, 0, 0]))
00359             position = list(translation_from_matrix(target_mat))
00360             rpy = list(euler_from_matrix(target_mat))
00361 
00362         # If no RPY specified, give position and return the method.
00363         if not rpy:
00364             try:
00365                 movegr.set_position_target(position)
00366             except MoveItCommanderException as e:
00367                 rospy.logerr(str(e))
00368             (movegr.go(do_wait) or movegr.go(do_wait) or
00369              rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))
00370             return
00371 
00372         # Not necessary to convert from rpy to quaternion, since
00373         # MoveGroupCommander.set_pose_target can take rpy format too.
00374         pose = copy.deepcopy(position)
00375         pose.extend(rpy)
00376         rospy.loginfo('setpose jntgr={} mvgr={} pose={} posi={} rpy={}'.format(
00377                       joint_group, movegr, pose, position, rpy))
00378         try:
00379             movegr.set_pose_target(pose)
00380         except MoveItCommanderException as e:
00381             rospy.logerr(str(e))
00382         except Exception as e:
00383             rospy.logerr(str(e))
00384 
00385         (movegr.go(do_wait) or movegr.go(do_wait) or
00386          rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group)))


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37