def on_enter(self, userdata): # reset timestamp self._timestamp = rospy.Time.now() self._activity_timestamp = self._timestamp # fill joint goal array self._goal_position = {} # get current joints positions joints_msg = self._joints_subscriber.get_last_msg(self._joints_topic) # check message if joints_msg == None or len(joints_msg.name) != len (joints_msg.position): Logger.logerr('JoystickJointControl: robot pose is unavailable or incorrect on topic `%s`' % self._joints_topic) raise RuntimeError('JoystickJointControl: robot pose is unavailable or incorrect on topic `%s`' % self._joints_topic) for event in self._buttons + self._axes: # find coresponding joint is_found = False for i in range(0,len(joints_msg.name)): if joints_msg.name[i] == event.joint: is_found = True self._goal_position[event.joint] = clip(joints_msg.position[i], event.min, event.max) # check if joint is found if not is_found: Logger.logerr('JoystickJointControl: unable to determine `%s` joint position. (min + max)/2 is used.' % event.joint) self._goal_position[event.joint] = (event.min + event.max) / 2.0 # default value Logger.loginfo('JoystickJointControl: start monitoring.')
def __init__(self, desired_tilt): '''Constructor''' super(TiltHeadState, self).__init__(outcomes=['done', 'failed']) if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") # self._configs['flor']['same'] = { # 20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees # 21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'}, # 22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'}, # 23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'}, # 24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'}, # 25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'} # max +60 degrees # } self._action_topic = "/" + controller_namespace + \ "/neck_traj_controller" + "/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) # Convert desired position to radians self._desired_tilt = math.radians(desired_tilt) self._failed = False
def execute(self, _): if self._target not in self.target_map.keys(): Logger.logerr('Target ' + str(self._target) + ' not found') return 'target_not_found' elapsed_time = rospy.get_rostime() - self._start_time if (elapsed_time.to_sec() < self._duration) or not self._use_timeout: # Logger.loghint('gazing at ' + str(self._target)) tar = GazeRelayTarget() tar.person_id = 0 tar.gaze_target = self.target_map[self._target] tar.force = self._force tar.min_duration = self._duration self._pub.publish(self._gaze_topic, tar) else: if not self._timeout_reached_print: Logger.loginfo('gazing at ' + str(self._target) + ' reached timeout!') self._timeout_reached_print = True # Logger.loginfo('gazing at ' + str(self._target) + ' reached timeout! publishing neutral target.') # tar = GazeRelayTarget() # tar.person_id = 0 # tar.gaze_target = self.target_map['neutral'] # self._pub.publish(self._gaze_topic, tar) return 'done'
def __init__(self, direction): ''' Constructor ''' super(FootstepPlanRelativeState, self).__init__(outcomes=['planned', 'failed'], input_keys=['distance'], output_keys=['plan_header']) if not rospy.has_param("behavior/step_distance_forward"): Logger.logerr("Need to specify parameter behavior/step_distance_forward at the parameter server") return if not rospy.has_param("behavior/step_distance_sideward"): Logger.logerr("Need to specify parameter behavior/step_distance_sideward at the parameter server") return self._step_distance_forward = rospy.get_param("behavior/step_distance_forward") self._step_distance_sideward = rospy.get_param("behavior/step_distance_sideward") self._action_topic = '/vigir/footstep_manager/step_plan_request' self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction}) self._done = False self._failed = False self._direction = direction
def on_exit(self, userdata): if self._action_topic in ProxyActionClient._result: ProxyActionClient._result[self._action_topic] = None if self._client.is_active(self._action_topic): Logger.logerr('%s Canceling active goal' % (self.name)) self._client.cancel(self._action_topic)
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '[MavrosSetMode]: ProxyServiceCaller: Topic \'{}\' not yet registered!' .format(self._service_topic)) self._failed = True return try: service_request = mavros_msgs.srv.SetModeRequest() service_request.custom_mode = self._request service_result = self._srv.call(self._service_topic, service_request) if not service_result.mode_sent: self._failed = True Logger.logwarn( '[MavrosSetMode]: Calling \'{}\' was not successful: {}'. format(self._service_topic, str(service_result.result))) else: Logger.loginfo( '[MavrosSetMode]: Mavros mode set to: {}'.format( self._request)) except Exception as e: Logger.logerr( '[MavrosSetMode]: Failed to call \'{}\' service request: {}'. format(self._service_topic, str(e))) self._failed = True
def __init__(self, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault", drake_sample_rate=4.0, drake_orientation_type=2, drake_link_axis=[0, 0, 1]): '''Constructor''' super(PlanAffordanceState, self).__init__( outcomes=['done', 'incomplete', 'failed'], input_keys=['affordance', 'hand', 'reference_point'], output_keys=['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() try: self.set_up_affordance_service() # topics and clients are set here except Exception as e: Logger.logerr( 'Failed to set up GetAffordanceInWristFrame service: \n%s' % str(e)) return self._vel_scaling = vel_scaling self._planner_id = planner_id self._drake_sample_rate = drake_sample_rate self._drake_orientation_type = drake_orientation_type self._drake_link_axis = Point(x=drake_link_axis[0], y=drake_link_axis[1], z=drake_link_axis[2]) self._failed = False self._done = False self._incomplete = False
def execute(self, userdata): ''' Execute this state ''' if self._failed: userdata.plan_header = None return 'failed' if self._done: return 'planned' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.warning != ErrorStatus.NO_WARNING: Logger.logwarn('Planning footsteps warning:\n%s' % result.status.warning_msg) if result.status.error == ErrorStatus.NO_ERROR: userdata.plan_header = result.step_plan.header num_steps = len(result.step_plan.steps) Logger.loginfo('Received plan with %d steps' % num_steps) self._done = True return 'planned' else: userdata.plan_header = None Logger.logerr('Planning footsteps failed:\n%s' % result.status.error_msg) self._failed = True return 'failed'
def execute(self, userdata): if self._failed: userdata.plan_header = None return 'failed' if self._done: return 'planned' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.warning != ErrorStatus.NO_WARNING: Logger.logwarn('Planning footsteps warning:\n%s' % result.status.warning_msg) if result.status.error == ErrorStatus.NO_ERROR: userdata.plan_header = result.step_plan.header num_steps = len(result.step_plan.steps) Logger.loginfo('Received plan with %d steps' % num_steps) self._done = True return 'planned' else: userdata.plan_header = None # as recommended: dont send out incomplete plan Logger.logerr('Planning footsteps failed:\n%s' % result.status.error_msg) self._failed = True return 'failed'
def on_enter(self, userdata): '''Upon entering the state, create and send the action goal message''' self._failed = False if userdata.hand_side not in self._hands_in_use: Logger.logerr( 'Hand side from userdata (%s) does not match hands in use: %s' % (userdata.hand_side, self._hands_in_use)) self._failed = True return # Create goal message goal = FollowJointTrajectoryGoal() goal.trajectory = userdata.finger_trajectory # Send goal to action server for execution try: self._active_topic = self._client_topics[userdata.hand_side] self._client.send_goal(self._active_topic, goal) except Exception as e: Logger.logwarn( 'Failed to send follow (hand) joint trajectory action goal:\n%s' % str(e)) self._failed = True
def __init__(self, config_name, move_group="", action_topic='/move_group', robot_name=""): ''' Constructor ''' super(SrdfStateGetJoints, self).__init__(outcomes=['done', 'failed', 'param_error'], output_keys=[ 'config_name', 'move_group', 'action_topic', 'robot_name', 'joint_values', 'joint_names' ]) self._config_name = config_name self._move_group = move_group self._robot_name = robot_name self._action_topic = action_topic self._srdf_param = None if rospy.has_param("/robot_description_semantic"): self._srdf_param = rospy.get_param("/robot_description_semantic") else: Logger.logerr( 'Unable to get parameter: /robot_description_semantic') self._param_error = False self._failed = False self._srdf = None
def __init__(self, timeout=5.0, max_delay=-1.0, wait_duration=5, action_topic=None): ''' Constructor ''' super(ExecuteKnownTrajectoryState, self).__init__( input_keys=['action_topic', 'trajectory'], outcomes=['done', 'failed', 'param_error'], output_keys=['status_text', 'goal_names', 'goal_values'] ) self.action_client = None self.goal_names = None self.goal_values = None self.timeout_duration = rospy.Duration(timeout) self.timeout_target = None self.wait_duration = wait_duration self.given_action_topic = action_topic self.max_delay = None if max_delay > 0.0: self.max_delay = rospy.Duration(max_delay) try: if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0): # If topic is defined, set the client up on startup self.action_client = ProxyActionClient({self.given_action_topic: ExecuteKnownTrajectoryAction}, self.wait_duration) else: self.given_action_topic = None # override "" except Exception as e: Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e))) self.current_action_topic = self.given_action_topic self.status_text = None self.return_code = None
def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'error' # Check if the action has been finished if self._client.has_result(self._topic): status = self._client.get_state(self._topic) result = self._client.get_result(self._topic) Logger.loginfo("[LidarFlierAction]: message %s" % str(result.message)) if status == GoalStatus.SUCCEEDED: Logger.loginfo("[LidarFlierAction]: %s" % str(result.message)) return self.result_to_outcome(result.result_id) elif status == GoalStatus.PREEMPTED: Logger.logwarn('[LidarFlierAction]: Action preempted') return 'preempted' elif status in [ GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED ]: Logger.logerr('[LidarFlierAction]: Action failed: %s' % str(result.message)) return self.result_to_outcome(result.result_id) """feedback params are""" feedback = self._client.get_feedback(self._topic) if feedback is not None: Logger.loginfo("[LidarFlierAction]: Feedback msg: %3d" % feedback.progress)
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False Logger.loginfo("[ServiceSetBrickPlacedIntoPlanKeeper]: entering state") # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr('[ServiceSetBrickPlacedIntoPlanKeeper]: Topic \'{}\' not yet registered!'.format(self._service_topic)) self._failed = True return try: service_request = plan_keeper.srv.BrickResultRequest() service_request.brick_id = userdata.brick_id service_request.result = userdata.placing_result Logger.loginfo("[ServiceSetBrickPlacedIntoPlanKeeper]: calling service %s"%(str(self._service_topic))) service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('[ServiceSetBrickPlacedIntoPlanKeeper]: Calling \'{}\' was not successful: {}'.format(self._service_topic, str(service_result.message))) else: Logger.loginfo('[ServiceSetBrickPlacedIntoPlanKeeper]: service result is %s'%(str(service_result.message))) except Exception as e: Logger.logerr('[ServiceSetBrickPlacedIntoPlanKeeper]: Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e))) self._failed = True
def checkHandPosition(self, input_pose): ''' Check if hand is in good position for brohoof. Return 'good' or 'bad'. ''' # check if frame header presents if not input_pose.header.frame_id: Logger.logerr('checkHandPosition: Header is missing.') return 'bad' # convert ot base_link frame try: output_pose = self._tf.transformPose('base_link', PoseStamped(Header(frame_id = input_pose.header.frame_id), input_pose.pose)) except tf.Exception as e: Logger.logerr('calculateHeadPoseFunc: Cannot transform from `%s` to `base_link` frame.' % input_pose.header.frame_id) return 'bad' pos = output_pose.pose.position Logger.loginfo('calculateHeadPoseFunc: object pos: %s' % str(pos)) # Check if boundarie are good if pos.z > self.brohoof_upper_limit or pos.z < -0.15: # too high or to low return 'bad' if pos.x < 0.25: # too close return 'bad' # Move to shoulder1 point pos.x -= 0.080 pos.y -= 0.037 pos.z += 0.027 # Check if angle is out of cone if abs(math.atan2(pos.y,pos.x)) > self.brohoof_cone: # out of cone return 'bad' return 'good'
def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if not self._connected: Logger.logerr('[ServiceGoToState]: not connected to %s' % self._control_manager_diagnostics_topic) return 'failed' diff_time = rospy.get_time() - self._start_time if self._sub_cont_diag.has_msg( self._control_manager_diagnostics_topic): Logger.loginfo( '[ServiceGoToState]: has_diagnostics msg at time %s' % (str(rospy.get_time()))) message = self._sub_cont_diag.get_last_msg( self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg( self._control_manager_diagnostics_topic) if diff_time >= 1.0 and not message.tracker_status.have_goal: Logger.loginfo( '[ServiceGoToState]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic) return 'successed' else: return if self._failed: return 'failed'
def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: Logger.logerr('[PlacingAction]: ending with "placed"') return 'placing_error' # Check if the action has been finished if self._client.has_result(self._topic): result = self._client.get_result(self._topic) Logger.loginfo("[PlacingAction]: ended with result %s" % str(result.message)) if result.success: Logger.loginfo('[PlacingAction]: ending with "placed"') userdata.placing_result = True return 'placed' else: Logger.logwarn('[PlacingAction]: ending with "placing_error"') userdata.placing_result = False return 'placing_error' """feedback params are""" feedback = self._client.get_feedback(self._topic) if feedback is not None: rospy.loginfo_throttle( 5, "[PlacingAction]: Current feedback msg: %s" % (str(feedback.message)))
def execute(self, userdata): if (self.return_code is None): # This should have been set by on_enter Logger.logerr("Invalid handling of SRDF data!") self.return_code = 'param_error' return self.return_code
def execute(self, userdata): if (self.return_code is not None): # Handle blocked transition or error during on_enter userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values return self.return_code while self.sub.has_buffered(self.topic): msg = self.sub.get_from_buffer(self.topic) for i in range(len(msg.name)): if msg.name[i] in self.joint_names \ and self.joint_values[self.joint_names.index(msg.name[i])] is None: self.joint_values[self.joint_names.index( msg.name[i])] = msg.position[i] if all(v is not None for v in self.joint_values): userdata.joint_names = self.joint_names userdata.joint_values = self.joint_values self.return_code = 'retrieved' return 'retrieved' if (self.timeout_duration is not None and \ (rospy.Time.now()-self.start_time) > self.timeout_duration ): Logger.logerr( 'Timeout %s - found %s of %s' % (self.name, str(self.joint_values), str(self.joint_names))) self.return_code = 'timeout' return 'timeout'
def on_enter(self, userdata): self.return_code = None try: if (len(userdata.joint_names) != len(userdata.joint_values)): Logger.logerr( 'Mismatch in joint names and values (%d vs. %d) -' % (len(userdata.joint_values), len(self.joint_names))) self.return_code = 'param_error' return # Action Initialization joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.Time.now() joint_trajectory.joint_names = [ name for name in userdata.joint_names ] joint_trajectory.points = [JointTrajectoryPoint()] joint_trajectory.points[0].time_from_start = rospy.Duration( self.duration) joint_trajectory.points[0].positions = [ value for value in userdata.joint_values ] # Assign to the user data userdata.joint_trajectory = joint_trajectory self.return_code = 'done' except Exception as e: Logger.logerr( 'Unable to convert joint values to trajectory - \n%s' % str(e)) self.return_code = 'param_error'
def execute(self, userdata): ''' Execute this state ''' if not self._connected: return 'failed' if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) self._sub.remove_last_msg(self._topic) try: if self._function(message): self._output_function(message, userdata) return 'successed' else: return except Exception as e: Logger.logerr('[WaitForMsg]: Failed to use function. Error is: %s' % str(e)) return 'failed' if self._wait_time >= 0: elapsed = rospy.get_rostime() - self._start_time; if (elapsed.to_sec() > self._wait_time): Logger.logerr('[WaitForMsg]: Condition was not satisfied in time limit %d s!'% self._wait_time) return 'failed'
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr( '{}ProxyServiceCaller: Topic \'{}\' not yet registered!'. format(self._state_name, self._service_topic)) self._failed = True return try: service_request = mrs_msgs.srv.StringRequest() service_request.value = self._msg_text service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn( '{}Calling \'{}\' was not successful: {}'.format( self._state_name, self._service_topic, str(service_result.message))) else: Logger.loginfo(self._state_name + str(service_result.message)) except Exception as e: Logger.logerr('{}Failed to call \'{}\' service request: {}'.format( self._state_name, self._service_topic, str(e))) self._failed = True
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._failed = False self._placeable = False # Check if the ProxyServiceCaller has been registered if not self._srv.is_available(self._service_topic): Logger.logerr('ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(self._service_topic)) self._failed = True return try: service_request = plan_keeper.srv.IsBrickPlaceableRequest() service_request.brick_id = userdata.brick_id Logger.loginfo('ask if brick %d is placeable'%(userdata.brick_id)) service_result = self._srv.call(self._service_topic, service_request) if not service_result.success: self._failed = True Logger.logwarn('Calling \'{}\' was not successful: {}'.format(self._service_topic, str(service_result.message))) else: Logger.loginfo(service_result.message) self._placeable = service_result.placeable except Exception as e: Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic, str(e))) self._failed = True
def __init__(self, direction): ''' Constructor ''' super(FootstepPlanRelativeState, self).__init__(outcomes=['planned', 'failed'], input_keys=['distance'], output_keys=['plan_header']) if not rospy.has_param("behavior/step_distance_forward"): Logger.logerr( "Need to specify parameter behavior/step_distance_forward at the parameter server" ) return if not rospy.has_param("behavior/step_distance_sideward"): Logger.logerr( "Need to specify parameter behavior/step_distance_sideward at the parameter server" ) return self._step_distance_forward = rospy.get_param( "behavior/step_distance_forward") self._step_distance_sideward = rospy.get_param( "behavior/step_distance_sideward") self._action_topic = '/vigir/footstep_manager/step_plan_request' self._client = ProxyActionClient( {self._action_topic: StepPlanRequestAction}) self._done = False self._failed = False self._direction = direction
def execute(self, userdata): """ Execute this state """ if self._failed: userdata.plan_header = None return "failed" if self._done: return "planned" if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.status.warning != ErrorStatus.NO_WARNING: Logger.logwarn("Planning footsteps warning:\n%s" % result.status.warning_msg) if result.status.error == ErrorStatus.NO_ERROR: userdata.plan_header = result.step_plan.header num_steps = len(result.step_plan.steps) Logger.loginfo("Received plan with %d steps" % num_steps) self._done = True return "planned" else: userdata.plan_header = None Logger.logerr("Planning footsteps failed:\n%s" % result.status.error_msg) self._failed = True return "failed"
def execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. #Logger.logerr('[ServiceFollowTrajectory]: execute') if not self._connected: Logger.logerr('[ServiceFollowTrajectory]: not connected to %s' % self._control_manager_diagnostics_topic) return 'failed' diff_time = rospy.get_time() - self._start_time if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic): rospy.loginfo_throttle(5,'[ServiceFollowTrajectory]: has diagnostics msg at time %s' % (str(rospy.get_time()))) message = self._sub_cont_diag.get_last_msg(self._control_manager_diagnostics_topic) self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic) if diff_time >= 1.0 and not message.tracker_status.tracking_trajectory: Logger.loginfo('[ServiceFollowTrajectory]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic) return 'successed' else: return if self._failed or diff_time >= len(userdata.scanning_trajectory) * 0.2 * 1.2: Logger.logerr('Failed follow trajectory \'{}\' within time {} s out of {} s'.format(self._service_topic_follow, str(diff_time),len(userdata.scanning_trajectory) * 0.2 * 1.2)) return 'failed'
def execute(self, userdata): if self._subscriber_pos.has_msg(self._topic): self.my_pose = self._subscriber_pos.get_last_msg(self._topic) if self._sub.has_msg('/entities'): Logger.loginfo('getting message') self.message = self._sub.get_last_msg('/entities') self._sub.remove_last_msg('/entities') if self.message is not None and self.my_pose is not None: for person in self.message.entities: if person.name == "person": result = self.add_person(person) # If people already exist if result == 1: result = self.update_person(person) # Error during person update. if result != 0: Logger.logerr("An error happen during people update.") # Error during person add. elif result < 0: Logger.logerr("An error happen during people add.") return 'done'
def determine_hands_in_use(self): '''Determine which hand types and sides have been sourced''' if not rospy.has_param("behavior/hand_type_prefix"): Logger.logerr( "Need to specify behavior/parameter hand_type_prefix at the parameter server" ) return hand_type_prefix = rospy.get_param("behavior/hand_type_prefix") hands_in_use = list() LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE'] RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE'] if LEFT_HAND not in ['l_stump', 'l_hook']: hands_in_use.append('left') if RIGHT_HAND not in ['r_stump', 'r_hook']: hands_in_use.append('right') if len(hands_in_use) == 0: Logger.logerr( 'No hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (LEFT_HAND, RIGHT_HAND)) return hands_in_use
def __init__(self, timeout=5.0, wait_duration=5, action_topic=None): ''' Constructor ''' super(ApplyPlanningSceneState, self).__init__( input_keys=['action_topic'], outcomes=['done', 'failed']) self.client = None self.timeout_duration = rospy.Duration(timeout) self.wait_duration = wait_duration self.action_topic = action_topic if (self.action_topic is not None) and (len(self.action_topic) > 0): # If topic is defined, set the client up on startup self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction}, self.wait_duration) else: self.action_topic = None # override "" self.moveit_client = None try: self.moveit_client = ProxyMoveItClient(None) except Exception as e: Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e))) self.success = None self.return_code = None
def execute(self, userdata): # While this state is active, check if the action has been finished and evaluate the result. # Check if the client failed to send the goal. if self._error: return 'aborted' # Check if the action has been finished if self._client.has_result(self._topic): status = self._client.get_state(self._topic) result = self._client.get_result(self._topic) if status == GoalStatus.SUCCEEDED: Logger.loginfo("[WallFollowing]: %s" % str(result.message)) return 'successed' elif status == GoalStatus.PREEMPTED: Logger.logwarn('[WallFollowing]: Action preempted') return 'preempted' elif status in [GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED]: Logger.logerr('[WallFollowing]: Action failed: %s' % str(result.message)) return 'aborted' """feedback params are""" feedback = self._client.get_feedback(self._topic) if feedback is not None: Logger.loginfo("[WallFollowing]: Current feedback msg: %s" % feedback.message)
def getEntities(self, name, containers): # Generate URL to contact if type(name) is str: url = "http://wonderland:8000/api/entity?entityClass=" + str(name) else: url = "http://wonderland:8000/api/entity?none" if type(containers) is str: url += "&entityContainer=" + containers elif type(containers) is list: for container in containers: if type(container) is str: url += "&entityContainer=" + container # try the request try: response = requests.get(url) except requests.exceptions.RequestException as e: Logger.logerr(e) return 'error' # parse parameter json data data = json.loads(response.content) entities = [] for entityData in data: if 'entityId' in entityData: entities.append(self.generateEntity(entityData)) return entities
def execute(self, userdata): if (self.return_code is None): # This should have been set by on_enter Logger.logerr("Invalid handling of data!") self.return_code = 'param_error' # Return the code determined when entering the state return self.return_code
def recoverPath(self, local_target, local_prior, pv, qv): pp = pv.x * pv.x + pv.y * pv.y qq = qv.x * qv.x + qv.y * qv.y qp = -qv.x * pv.x - qv.y * pv.y # negate qv dp = qp / pp # fractional distance along the pv vector # Assume we steer toward the initial point control = PointStamped() control.header = local_prior.header control.point = deepcopy(local_prior.point) if (dp > 1.0): # Steer toward the target point control = local_target elif (dp > 0.0): # Steer toward the closest point control.point.x = control.point.x + dp * pv.x # Control point in the odometry frame control.point.y = control.point.y + dp * pv.y # Control point in the odometry frame control.point.z = control.point.z + dp * pv.z # Control point in the odometry frame self._reference_marker.pose.position = deepcopy(control.point) self._local_target_marker.pose.position = deepcopy(local_target.point) control_robot = self.transformBody(control) if (control_robot.point.x <= 0.001): control_robot = self.transformBody(local_target) # One last try if (control_robot.point.x <= 0.001): dist = control_robot.point.x * control_robot.point.x + control_robot.point.y * control_robot.point.y if (dist > 2.5): Logger.loginfo( "recovery control point is behind the robot and far way abort recovery!" ) return False else: # Target is close enough - do a zero radius turn toward the target line until our closet point is ahead self._twist.twist.linear.x = 0.0 self._twist.twist.angular.z = math.copysign( self._desired_velocity / 0.13, control_robot.point.y) return True else: # Target is ahead - do a zero radius turn toward the target line until our closet point is ahead self._twist.twist.linear.x = 0.0 self._twist.twist.angular.z = math.copysign( self._desired_velocity / 0.13, control_robot.point.y) return True # Assume lookahead tangent to the control point dc = Vector3(control.point.x - self._current_position.point.x, control.point.y - self._current_position.point.y, 0.0) curvature = 2.0 * control_robot.point.y / (dc.x * dc.x + dc.y * dc.y) if (np.isnan(curvature)): Logger.logerr("invalid curvature calculation abort recovery!") return False self._twist.twist.angular.z = curvature * self._desired_velocity return True
def _is_switchable(self, be): if self.be.name != be.name: Logger.logerr('Unable to switch behavior, names do not match:\ncurrent: %s <--> new: %s' % (self.be.name, be.name)) return False # locked inside # locked state exists in new behavior #if self.be.id == be.id: #Logger.logwarn('Behavior version ID is the same.') # Logger.logwarn('Skipping behavior switch, version ID is the same.') # return False # ok, can switch return True
def __init__(self, arm_side, target_config, time=2.0): """Constructor""" super(GotoSingleArmJointConfigState, self).__init__(outcomes=["done", "failed"], input_keys=["current_config"]) if not rospy.has_param("behavior/robot_namespace"): Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server") return self._robot = rospy.get_param("behavior/robot_namespace") if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") ################################ ATLAS ################################ self._configs = dict() self._configs["flor"] = dict() self._configs["flor"]["left"] = { 11: {"joint_name": "l_arm_wry2", "joint_value": -2.5}, 12: {"joint_name": "l_arm_wry2", "joint_value": +2.5}, } self._configs["flor"]["right"] = { 11: {"joint_name": "r_arm_wry2", "joint_value": +2.5}, 12: {"joint_name": "r_arm_wry2", "joint_value": -2.5}, } ################################ THOR ################################# self._configs["thor_mang"] = dict() self._configs["thor_mang"]["left"] = { 11: {"joint_name": "l_wrist_yaw2", "joint_value": 3.84}, 12: {"joint_name": "l_wrist_yaw2", "joint_value": -3.84}, } self._configs["thor_mang"]["right"] = { 11: {"joint_name": "r_wrist_yaw2", "joint_value": -3.84}, 12: {"joint_name": "r_wrist_yaw2", "joint_value": 3.84}, } ####################################################################### self._joint_name = self._configs[self._robot][arm_side][target_config]["joint_name"] self._joint_value = self._configs[self._robot][arm_side][target_config]["joint_value"] self._time = time self._action_topic = ( "/" + controller_namespace + "/" + arm_side + "_arm_traj_controller" + "/follow_joint_trajectory" ) self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._failed = False
def __init__(self, controllers = ["left_arm_traj_controller", "right_arm_traj_controller", "torso_traj_controller", "left_leg_traj_controller", "right_leg_traj_controller"]): '''Constructor''' super(ExecuteTrajectoryWholeBodyState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['trajectories']) if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") self._controllers = controllers self._controllers = ["torso_traj_controller", "left_leg_traj_controller", "right_leg_traj_controller", "left_arm_traj_controller", "right_arm_traj_controller"] if not(controllers) else controllers self._client = ProxyActionClient() self._client_topics = dict() self._active_topics = list() for controller in self._controllers: if "left_arm" in controller: action_topic_left = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory" self._client.setupClient(action_topic_left, FollowJointTrajectoryAction) self._client_topics['left_arm'] = action_topic_left elif "right_arm" in controller: action_topic_right = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory" self._client.setupClient(action_topic_right, FollowJointTrajectoryAction) self._client_topics['right_arm'] = action_topic_right elif "left_leg" in controller: action_topic_left = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory" self._client.setupClient(action_topic_left, FollowJointTrajectoryAction) self._client_topics['left_leg'] = action_topic_left elif "right_leg" in controller: action_topic_right = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory" self._client.setupClient(action_topic_right, FollowJointTrajectoryAction) self._client_topics['right_leg'] = action_topic_right elif "torso" in controller: action_topic_right = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory" self._client.setupClient(action_topic_right, FollowJointTrajectoryAction) self._client_topics['torso'] = action_topic_right else: Logger.logwarn('ExecuteTrajectoryWholeBodyState:The controller is neither an arm, leg or torso trajectory controller!? %s' % str(controller)) print self._client_topics print self._client self._failed = False
def __init__(self, controller, joint_names): ''' Constructor ''' super(ExecuteTrajectoryState, self).__init__(outcomes=['done', 'failed'], input_keys=['joint_positions', 'time']) if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") self._joint_names = joint_names self._action_topic = "/" + controller_namespace + "/" + controller + "/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) self._failed = False
def __init__(self, target_mode): ''' Constructor ''' super(ChangeControlModeActionState, self).__init__(outcomes=['changed','failed']) if not rospy.has_param("behavior/mode_controllers_name"): Logger.logerr("Need to specify parameter behavior/mode_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/mode_controllers_name") self._action_topic = "/" + controller_namespace + "/control_mode_controller/change_control_mode" self._target_mode = target_mode self._client = ProxyActionClient({self._action_topic: ChangeControlModeAction}) self._failed = False
def __init__(self, side, controller = 'traj_controller'): ''' Constructor ''' super(QueryJointPositionsState, self).__init__(outcomes = ['retrieved', 'failed'], output_keys = ['joint_config']) if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") self._srv_topic = '/' + controller_namespace + '/' + side + '_arm_' + \ controller + '/query_state' self._srv = ProxyServiceCaller({self._srv_topic: QueryTrajectoryState}) self._srv_result = None self._failed = False
def on_enter(self, userdata): '''Upon entering the state, create and send the action goal message''' self._failed = False if userdata.hand_side not in self._hands_in_use: Logger.logerr('Hand side from userdata (%s) does not match hands in use: %s' % (userdata.hand_side, self._hands_in_use)) self._failed = True return # Create goal message goal = FollowJointTrajectoryGoal() goal.trajectory = userdata.finger_trajectory # Send goal to action server for execution try: self._active_topic = self._client_topics[userdata.hand_side] self._client.send_goal(self._active_topic, goal) except Exception as e: Logger.logwarn('Failed to send follow (hand) joint trajectory action goal:\n%s' % str(e)) self._failed = True
def determine_hands_in_use(self): '''Determine which hand types and sides have been sourced''' if not rospy.has_param("behavior/hand_type_prefix"): Logger.logerr("Need to specify behavior/parameter hand_type_prefix at the parameter server") return hand_type_prefix = rospy.get_param("behavior/hand_type_prefix") hands_in_use = list() LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE'] RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE'] if LEFT_HAND not in ['l_stump', 'l_hook']: hands_in_use.append('left') if RIGHT_HAND not in ['r_stump', 'r_hook']: hands_in_use.append('right') if len(hands_in_use) == 0: Logger.logerr('No hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (LEFT_HAND, RIGHT_HAND)) return hands_in_use
def __init__(self, vel_scaling = 0.1, planner_id = "RRTConnectkConfigDefault", drake_sample_rate = 4.0, drake_orientation_type = 2, drake_link_axis = [0, 0, 1]): '''Constructor''' super(PlanAffordanceState, self).__init__(outcomes = ['done', 'incomplete', 'failed'], input_keys = ['affordance', 'hand', 'reference_point'], output_keys = ['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() try: self.set_up_affordance_service() # topics and clients are set here except Exception as e: Logger.logerr('Failed to set up GetAffordanceInWristFrame service: \n%s' % str(e)) return self._vel_scaling = vel_scaling self._planner_id = planner_id self._drake_sample_rate = drake_sample_rate self._drake_orientation_type = drake_orientation_type self._drake_link_axis = Point( x = drake_link_axis[0], y = drake_link_axis[1], z = drake_link_axis[2] ) self._failed = False self._done = False self._incomplete = False
def __init__(self, hand_type): '''Constructor''' super(HandTrajectoryState, self).__init__(outcomes=['done', 'failed'], input_keys=['finger_trajectory', 'hand_side']) if not rospy.has_param("behavior/hand_controllers_name"): Logger.logerr("Need to specify parameter behavior/hand_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/hand_controllers_name") if not rospy.has_param("behavior/hand_type_prefix"): Logger.logerr("Need to specify parameter behavior/hand_type_prefix at the parameter server") return hand_type_prefix = rospy.get_param("behavior/hand_type_prefix") # Determine which hand types and sides have been sourced self._hands_in_use = list() LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE'] RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE'] if LEFT_HAND == 'l_' + hand_type: self._hands_in_use.append('left') if RIGHT_HAND == 'r_' + hand_type: self._hands_in_use.append('right') if len(self._hands_in_use) == 0: Logger.logerr('No %s hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (hand_type, LEFT_HAND, RIGHT_HAND)) # Initialize the action clients corresponding to the hands in use self._client_topics = dict() self._active_topic = None self._client = ProxyActionClient() for hand_side in self._hands_in_use: action_topic = ("/%s/%s_hand_traj_controller/follow_joint_trajectory" % (controller_namespace, hand_side)) self._client.setupClient(action_topic, FollowJointTrajectoryAction) self._client_topics[hand_side] = action_topic self._failed = False
def _prepare_behavior(self, msg): # get sourcecode from ros package try: rp = rospkg.RosPack() behavior = self._behavior_lib[msg.behavior_id] be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py') if os.path.isfile(be_filepath): be_file = open(be_filepath, "r") rospy.logwarn("Found a tmp version of the referred behavior! Assuming local test run.") else: be_filepath = os.path.join(rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py') be_file = open(be_filepath, "r") be_content = be_file.read() be_file.close() except Exception as e: Logger.logerr('Failed to retrieve behavior from library:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # apply modifications if any try: file_content = "" last_index = 0 for mod in msg.modifications: file_content += be_content[last_index:mod.index_begin] + mod.new_content last_index = mod.index_end file_content += be_content[last_index:] if zlib.adler32(file_content) != msg.behavior_checksum: mismatch_msg = ("checksum mismatch of behavior versions! \n" "Attempted to load behavior: %s" % str(be_filepath)) raise Exception(mismatch_msg) else: rospy.loginfo("Successfully applied %d modifications." % len(msg.modifications)) except Exception as e: Logger.logerr('Failed to apply behavior modifications:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # create temp file for behavior class try: file_path = os.path.join(self._tmp_folder, 'tmp_%d.py' % msg.behavior_checksum) sc_file = open(file_path, "w") sc_file.write(file_content) sc_file.close() except Exception as e: Logger.logerr('Failed to create temporary file for behavior class:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import temp class file and initialize behavior try: package = __import__("tmp_%d" % msg.behavior_checksum, fromlist=["tmp_%d" % msg.behavior_checksum]) clsmembers = inspect.getmembers(package, lambda member: inspect.isclass(member) and member.__module__ == package.__name__) beclass = clsmembers[0][1] be = beclass() rospy.loginfo('Behavior ' + be.name + ' created.') except Exception as e: Logger.logerr('Unable to import temporary behavior file:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # import contained behaviors contain_list = {} try: contain_list = self._build_contains(be, "") except Exception as e: Logger.logerr('Failed to load contained behaviors:\n%s' % str(e)) return # initialize behavior parameters if len(msg.arg_keys) > 0: rospy.loginfo('The following parameters will be used:') try: for i in range(len(msg.arg_keys)): key_splitted = msg.arg_keys[i].rsplit('/', 1) behavior = key_splitted[0] key = key_splitted[1] found = False if behavior == '' and hasattr(be, key): self._set_typed_attribute(be, key, msg.arg_values[i]) found = True for b in contain_list: if hasattr(contain_list[b], key): self._set_typed_attribute(contain_list[b], key, msg.arg_values[i], b) found = True if not found: rospy.logwarn('Parameter ' + msg.arg_keys[i] + ' (set to ' + msg.arg_values[i] + ') not implemented') except Exception as e: Logger.logerr('Failed to initialize parameters:\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return # build state machine try: be.set_up(id=msg.behavior_checksum, autonomy_level=msg.autonomy_level, debug=False) be.prepare_for_execution() rospy.loginfo('State machine built.') except Exception as e: Logger.logerr('Behavior construction failed!\n%s' % str(e)) self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.ERROR)) return return be
def _behavior_execution(self, msg): if self._running: Logger.loginfo('--> Initiating behavior switch...') self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received'])) else: Logger.loginfo('--> Starting new behavior...') be = self._prepare_behavior(msg) if be is None: Logger.logerr('Dropped behavior start request because preparation failed.') if self._running: self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) else: rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') return if self._running: if self._switching: Logger.logwarn('Already switching, dropped new start request.') return self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start'])) if not self._is_switchable(be): Logger.logerr('Dropped behavior start request because switching is not possible.') self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable'])) return self._switching = True active_state = self.be.get_current_state() rospy.loginfo("Current state %s is kept active.", active_state.name) try: be.prepare_for_switch(active_state) self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared'])) except Exception as e: Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e)) self._switching = False self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) return rospy.loginfo('Preempting current behavior version...') self.be.preempt_for_switch() rate = rospy.Rate(10) while self._running: rate.sleep() self._switching = False self._running = True self.be = be result = "" try: rospy.loginfo('Behavior ready, execution starts now.') rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum) self.be.confirm() args = [self.be.requested_state_path] if self.be.requested_state_path is not None else [] self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args)) result = self.be.execute() if self._switching: self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING)) else: self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED)) except Exception as e: self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED)) Logger.logerr('Behavior execution failed!\n%s' % str(e)) result = "failed" try: self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) self.be = None if not self._switching: rospy.loginfo('Behavior execution finished with result %s.', str(result)) rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m') self._running = False
def _callback(self, msg): try: be_id, behavior = next((id, be) for (id, be) in self._behavior_lib.items() if be["name"] == msg.behavior_name) except Exception as e: Logger.logerr("Did not find behavior with requested name: %s" % msg.behavior_name) self._status_pub.publish(BEStatus(code=BEStatus.ERROR)) return rospy.loginfo("Request for behavior " + behavior["name"]) be_selection = BehaviorSelection() be_selection.behavior_id = be_id be_selection.autonomy_level = msg.autonomy_level try: for k, v in zip(msg.arg_keys, msg.arg_values): if k.startswith('/YAML:'): key = k.replace('/YAML:', '/', 1) path = v.split(':')[0] ns = v.split(':')[1] if path.startswith('~') or path.startswith('/'): yamlpath = os.path.expanduser(path) else: yamlpath = os.path.join(self._rp.get_path(path.split('/')[0]), '/'.join(path.split('/')[1:])) with open(yamlpath, 'r') as f: content = yaml.load(f) if ns != '' and ns in content: content = content[ns] be_selection.arg_keys.append(key) be_selection.arg_values.append(yaml.dump(content)) else: be_selection.arg_keys.append(k) be_selection.arg_values.append(v) except Exception as e: rospy.logwarn('Failed to parse and substitute behavior arguments, will use direct input.\n%s' % str(e)) be_selection.arg_keys = msg.arg_keys be_selection.arg_values = msg.arg_values be_structure = ContainerStructure() be_structure.containers = msg.structure try: be_filepath_new = os.path.join(self._rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '.py') except ResourceNotFound: rospy.logerr("Could not find behavior package '%s'" % (behavior["package"])) rospy.loginfo("Have you updated your ROS_PACKAGE_PATH after creating the behavior?") return with open(be_filepath_new, "r") as f: be_content_new = f.read() be_filepath_old = os.path.join(self._rp.get_path(behavior["package"]), 'src/' + behavior["package"] + '/' + behavior["file"] + '_tmp.py') if not os.path.isfile(be_filepath_old): be_selection.behavior_checksum = zlib.adler32(be_content_new) if msg.autonomy_level != 255: be_structure.behavior_id = be_selection.behavior_checksum self._mirror_pub.publish(be_structure) self._pub.publish(be_selection) rospy.loginfo("No changes to behavior version.") return with open(be_filepath_old, "r") as f: be_content_old = f.read() sqm = difflib.SequenceMatcher(a=be_content_old, b=be_content_new) diffs = [x[1] for x in sqm.get_grouped_opcodes(0)] for opcode, a0, a1, b0, b1 in diffs: content = be_content_new[b0:b1] be_selection.modifications.append(BehaviorModification(a0, a1, content)) be_selection.behavior_checksum = zlib.adler32(be_content_new) if msg.autonomy_level != 255: be_structure.behavior_id = be_selection.behavior_checksum self._mirror_pub.publish(be_structure) self._pub.publish(be_selection)
def __init__(self, target_pose, vel_scaling = 0.1, ignore_collisions = False, link_paddings = {}, is_cartesian = False): """Constructor""" super(MoveitPredefinedPoseState, self).__init__(outcomes=['done', 'failed'], input_keys=['side']) if not rospy.has_param("behavior/robot_namespace"): Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server") return self._robot = rospy.get_param("behavior/robot_namespace") self._poses = dict() self._poses['flor'] = dict() # Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch self._poses['flor']['left'] = { # Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2 1: {'group': 'l_arm_group', 'joints': [-0.25, -1.34, 1.88, 0.49, 0.0, 0.0, 0.0]}, 3: {'group': 'l_arm_group', 'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4, -0.50]}, 10: {'group': 'l_arm_group', 'joints': [-1.0, 0.28, 1.2, 1.6, 0.3, 0.5 , 0.0]}, 21: {'group': 'torso_group', 'joints': [+0.20, 0.00, 0.00]}, 22: {'group': 'torso_group', 'joints': [+0.40, 0.00, 0.00]}, 23: {'group': 'torso_group', 'joints': [+0.55, 0.00, 0.00]}, 112: {'group': 'l_leg_group', 'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]}, # Safety pose 114: {'group': 'l_arm_group', 'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]}, # pre_drive 115: {'group': 'l_arm_group', 'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]}, # drive 116: {'group': 'l_arm_group', 'joints': []}, # We use the right hand for the camera! 131: {'group': 'l_arm_group', 'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]}, 132: {'group': 'l_arm_group', 'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]}, 133: {'group': 'l_arm_group', 'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]}, 134: {'group': 'l_arm_group', 'joints': []}, # Most probably will never be used 151: {'group': 'l_arm_group', 'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]}, 155: {'group': 'l_arm_group', 'joints': [0.0, -0.37, 2.65, 1.4, -0.2, 0.9 , -1.54]}, 156: {'group': 'l_arm_group', 'joints': [-0.32, -0.9, 2.2, 1.3, 0.5, 1.0 , -1.8]}, 157: {'group': 'l_arm_group', 'joints': [-0.45, -1.0, 2.1, 1.3, 0.5, 0.8 , -0.8]} } self._poses['flor']['same'] = { 0: {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \ +0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]}, 2: {'group': 'both_arms_group', 'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4 , -0.5, \ -0.72, 0.95, 2.7, -0.95, 0.0, 0.4 , -0.5]}, 19: {'group': 'both_arms_group', 'joints': [-1.5, -1.5, +0.30, +0.50, +0.0, +0.8, +0.00, \ +1.5, +1.5, +0.30, -0.50, +0.0, -0.8, +0.00]}, 20: {'group': 'torso_group', 'joints': [+0.00, 0.00, 0.00]}, 111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \ +0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]}, 113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \ +0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]}, 181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \ +1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]} } self._poses['flor']['right'] = { # Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2 1: {'group': 'r_arm_group', 'joints': [+0.25, 1.34, 1.88, -0.49, 0.0, 0.0, 0.0]}, 3: {'group': 'r_arm_group', 'joints': [-0.72, 0.95, 2.7, -0.95, 0.0, 0.4, -0.50]}, 10: {'group': 'r_arm_group', 'joints': [+1.0, -0.28, 1.2, -1.6, 0.3, -0.5 , 0.0]}, 21: {'group': 'torso_group', 'joints': [-0.20, 0.00, 0.00]}, 22: {'group': 'torso_group', 'joints': [-0.40, 0.00, 0.00]}, 23: {'group': 'torso_group', 'joints': [-0.55, 0.00, 0.00]}, 112: {'group': 'r_leg_group', 'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]}, 115: {'group': 'r_arm_group', 'joints': []}, # Driving is done with the left arm! 116: {'group': 'r_arm_group', 'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]}, # CarCamera 131: {'group': 'r_arm_group', 'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]}, 132: {'group': 'r_arm_group', 'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]}, 133: {'group': 'r_arm_group', 'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]}, 134: {'group': 'r_arm_group', 'joints': [+0.00, +0.54, +0.94, -1.04, 0.80, 0.5, 0.7]}, 151: {'group': 'r_arm_group', 'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]}, 155: {'group': 'r_arm_group', 'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]}, 156: {'group': 'r_arm_group', 'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]}, 157: {'group': 'r_arm_group', 'joints': [0.45, 1.0, 2.1, -1.3, 0.5, -0.8 , -0.8]} } self._poses['thor_mang'] = dict() self._poses['thor_mang']['left'] = { 1: {'group': 'l_arm_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0]}, 3: {'group': 'l_arm_group', 'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]}, 4: {'group': 'l_arm_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.97080884130538, -0.25205140042963, 0.01563815008, 0]}, 21: {'group': 'torso_group', 'joints': [+0.30, 0.03]}, 22: {'group': 'torso_group', 'joints': [+0.60, 0.03]}, 23: {'group': 'torso_group', 'joints': [+1.02, 0.03]}, 31: {'group': 'head_group', 'joints': [1.57, 0.00]}, 131: {'group': 'l_arm_group', 'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]}, 132: {'group': 'l_arm_group', 'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]}, 133: {'group': 'l_arm_group', 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]}, 134: {'group': 'l_arm_group', 'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]}, # placeholder 136: {'group': 'l_arm_group', 'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]}, 138: {'group': 'torso_group', 'joints': [0.92, 0]}, 139: {'group': 'torso_group', 'joints': [1.3, 0]}, 151: {'group': 'l_arm_group', 'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]}, 155: {'group': 'l_arm_group', 'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]}, 156: {'group': 'l_arm_group', 'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]}, 157: {'group': 'l_arm_group', 'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]} } self._poses['thor_mang']['same'] = { 0: {'group': 'both_arms_group', 'joints': [0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57, -1.55, 0, 0]}, 2: {'group': 'both_arms_group', 'joints': [-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]}, 20: {'group': 'torso_group', 'joints': [0.00, 0.03]}, 30: {'group': 'head_group', 'joints': [0.00, 0.00]}, 181: {'group': 'both_arms_group', 'joints': [0.785385646194622, -0.281153767716932, 0.000600782658167331, -1.57080884130538, -0.25205140042963, 0.01563815008, 0, -0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]} } self._poses['thor_mang']['right'] = { 1: {'group': 'r_arm_group', 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.57080884130538, 0.25205140042963, -0.01563815008, 0]}, 3: {'group': 'r_arm_group', 'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]}, 4: {'group': 'r_arm_group', 'joints': [-0.785385646194622, 0.281153767716932, -0.000600782658167331, 1.97080884130538, 0.25205140042963, -0.01563815008, 0]}, 21: {'group': 'torso_group', 'joints': [-0.30, 0.03]}, 22: {'group': 'torso_group', 'joints': [-0.60, 0.03]}, 23: {'group': 'torso_group', 'joints': [-1.02, 0.03]}, 31: {'group': 'head_group', 'joints': [-1.57, 0.00]}, 131: {'group': 'r_arm_group', 'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]}, 132: {'group': 'r_arm_group', 'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]}, 133: {'group': 'r_arm_group', 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]}, 134: {'group': 'r_arm_group', 'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]}, # placeholder 136: {'group': 'r_arm_group', 'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]}, 138: {'group': 'torso_group', 'joints': [-0.92, 0]}, 139: {'group': 'torso_group', 'joints': [-1.3, 0]}, 151: {'group': 'r_arm_group', 'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]}, 155: {'group': 'r_arm_group', 'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]}, 156: {'group': 'r_arm_group', 'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]}, 157: {'group': 'r_arm_group', 'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]} } self._target_pose = target_pose self._vel_scaling = vel_scaling self._ignore_collisions = ignore_collisions self._link_paddings = link_paddings self._is_cartesian = is_cartesian self._client = ProxyMoveitClient() self._failed = False