def combine_plans(self, plans): output_dict = {} try: left_arm = plans[0] right_arm = plans[1] left_leg = plans[2] right_leg = plans[3] torso = plans[4] #hacky_dict_combination = lambda p: dict({'left_arm': p[0]}, **{'right_arm': p[1]}) # Logger.loginfo('SystemIdentificationTestsSM:combine_plans: plans="%s"' % str(plans)) if left_arm is not None: output_dict['left_arm'] = left_arm.joint_trajectory if right_arm is not None: output_dict['right_arm'] = right_arm.joint_trajectory if left_leg is not None: output_dict['left_leg'] = left_leg.joint_trajectory if right_leg is not None: output_dict['right_leg'] = right_leg.joint_trajectory if torso is not None: output_dict['torso'] = torso.joint_trajectory except Exception as e: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Error "%s"' % (str(e))) return output_dict
def on_enter(self, userdata): '''Upon entering the state, write trajectory goal(s) to action server(s)''' self._failed = False Logger.loginfo('ExecuteTrajectoryWholeBodyState: Received trajectory for %s.' % userdata.trajectories.keys()) # In execute, only check clients that have sent goals/trajectories self._active_topics = list() # Setup client and send goal to server for execution try: for chain, traj in userdata.trajectories.iteritems(): # Create goal message action_goal = FollowJointTrajectoryGoal() action_goal.trajectory = traj action_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) topic = self._client_topics[chain] self._client.send_goal(topic, action_goal) self._active_topics.append(topic) Logger.loginfo('ExecuteTrajectoryWholeBodyState: Goal message sent for %s (topic %s)' % (str(chain), str(topic))) except Exception as e: Logger.logwarn('ExecuteTrajectoryWholeBodyState: Failed to send follow joint trajectory action goal(s):\n%s' % str(e)) self._failed = True
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 set_back_rotation(self, input_keys): affordance = input_keys[0] back_rotation = input_keys[1] degrees = math.copysign(back_rotation, affordance.displacement) affordance.displacement = math.radians(degrees) Logger.loginfo("Turning back %.1f degrees" % degrees) return affordance
def execute(self, userdata): # execute all active states for name, state in self._states.items(): if self._returned_outcomes.has_key(name): continue outcome = None if isinstance(state, smach.StateMachine): rmp_ud = smach.Remapper( userdata, state.get_registered_input_keys(), state.get_registered_output_keys(), {key: name + '_' + key for key in state.get_registered_input_keys() + state.get_registered_output_keys()}) state.userdata.merge(rmp_ud, rmp_ud.keys(), dict()) with state._state_transitioning_lock: outcome = state._update_once() else: outcome = state.execute(smach.Remapper( userdata, state.get_registered_input_keys(), state.get_registered_output_keys(), {key: name + '_' + key for key in state.get_registered_input_keys() + state.get_registered_output_keys()})) if outcome is not None and outcome != "loopback": Logger.loginfo("Concurrency: %s > %s" % (name, outcome)) self._returned_outcomes[name] = outcome # determine outcome for element in self._outcome_mapping: state_outcome = element['outcome'] mapping = element['condition'] if all(name in self._returned_outcomes and self._returned_outcomes[name] is outcome for name, outcome in mapping.items()): return state_outcome
def execute(self, userdata): ''' Execute this state ''' if not self._connected: return 'no_connection' if self._received: return 'received' if self._client.has_result(self._action_topic): result = self._client.get_result(self._action_topic) if result.result_code != BehaviorInputResult.RESULT_OK: userdata.data = None return 'aborted' else: try: response_data = pickle.loads(result.data) #print 'Input request response:', response_data userdata.data = response_data # If this was a template ID request, log that template ID: if self._request == BehaviorInputGoal.SELECTED_OBJECT_ID: Logger.loginfo('Received template ID: %d' % int(response_data)) except Exception as e: Logger.logwarn('Was unable to load provided data:\n%s' % str(e)) userdata.data = None return 'data_error' self._received = True return 'received'
def on_enter(self, userdata): if not hasattr(userdata, 'rotation_axis') or userdata.rotation_axis is None \ or not hasattr(userdata, 'hand') or userdata.hand is None \ or not hasattr(userdata, 'rotation_angle') or userdata.rotation_angle is None: self._failed = True Logger.logwarn('Userdata key of state %s does not exist or is currently undefined!' % self.name) return center_pose = userdata.rotation_axis planning_group_str = \ ('l' if userdata.hand == self.LEFT_HAND else 'r') + \ '_arm' + \ ('_with_torso' if self._include_torso else '') + \ '_group' # create the motion goal self._client.new_goal(planning_group_str) self._client.set_collision_avoidance(self._ignore_collisions) self._client.set_circular_motion(userdata.rotation_angle) self._client.set_keep_orientation(self._keep_endeffector_orientation) self._client.add_endeffector_pose(userdata.rotation_axis.pose, userdata.rotation_axis.header.frame_id) # rotation center self._client.set_planner_id(self._planner_id) Logger.loginfo('Sending planning request for circular motion of %f rad...' % userdata.rotation_angle) try: self._client.start_planning() except Exception as e: Logger.logwarn('Could not request a plan!\n%s' % str(e)) self._failed = True
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 on_enter(self, userdata): self._sampling_failed = False self._planning_failed = False self._control_failed = False self._success = False action_goal = ArgoCombinedPlanGoal() action_goal.target = deepcopy(userdata.object_pose) type_map = { 'dial_gauge': ObjectTypes.DIAL_GAUGE, 'level_gauge': ObjectTypes.LEVEL_GAUGE, 'valve': ObjectTypes.VALVE, 'hotspot': ObjectTypes.HOTSPOT, 'pipes' : ObjectTypes.PIPES, 'victim' : ObjectTypes.VICTIM } object_type = type_map.get(userdata.object_type, ObjectTypes.UNKNOWN) q = [ action_goal.target.pose.orientation.x, action_goal.target.pose.orientation.y, action_goal.target.pose.orientation.z, action_goal.target.pose.orientation.w ] q = tf.transformations.quaternion_multiply(q, self._rot) action_goal.target.pose.orientation = Quaternion(*q) action_goal.object_id.data = userdata.object_id action_goal.object_type.val = object_type action_goal.action_type.val = ActionCodes.SAMPLE_MOVE_ARM Logger.loginfo('Position arm to look at %s object' % str(userdata.object_type)) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e)) self._control_failed = True
def on_exit(self, userdata): # Make sure that the action is not running when leaving this state. # A situation where the action would still be active is for example when the operator manually triggers an outcome. if not self._client.has_result(self._topic): self._client.cancel(self._topic) Logger.loginfo('Cancelled active action goal.')
def execute(self, userdata): if self._failed or self._srv_result is None: return 'failed' if self._done: return 'done' if self._not_available: return 'not_available' try: options = self._srv_result.pre_grasp_information.grasps if len(options) <= userdata.preference: Logger.logwarn('The option with index %d is not available. There are %d options total.' % (userdata.preference, len(options))) self._not_available = True return 'not_available' chosen_pregrasp = options[userdata.preference] userdata.pre_grasp = chosen_pregrasp.grasp_pose Logger.loginfo('Using grasp with ID: %s' % str(chosen_pregrasp.id)) except Exception as e: Logger.logwarn('Failed to retrieve grasp information from service result:\n%s' % str(e)) self._failed = True return 'failed' self._done = True return 'done'
def on_enter(self, userdata): self._failed = False listener = tf.TransformListener() transform = PoseStamped() if len(userdata.frames) != 2: self._failed = True Logger.logwarn('Did not receive two frames') return target_frame = userdata.frames[0] source_frame = userdata.frames[1] Logger.loginfo('Getting transform for target = %s, source = %s' % (target_frame, source_frame)) try: now = rospy.Time(0) listener.waitForTransform(target_frame, source_frame, now, rospy.Duration(4.0)) (trans,rot) = listener.lookupTransform(target_frame, source_frame, now) transform.pose.position.x = trans[0] transform.pose.position.y = trans[1] transform.pose.position.z = trans[2] transform.pose.orientation.x = rot[0] transform.pose.orientation.y = rot[1] transform.pose.orientation.z = rot[2] transform.pose.orientation.w = rot[3] userdata.transform = transform except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation:\n%s' % str(e)) self._failed = True userdata.transform = None return
def execute(self, userdata): if self._failed or self._srv_result is None: return 'failed' if self._done: return 'done' if self._not_available: return 'not_available' try: choices = self._srv_result.template_type_information.stand_poses Logger.loginfo("There are %d stand pose choices and want to select entry %d" % (len(choices), userdata.preference)) if len(choices) <= userdata.preference: self._not_available = True return 'not_available' userdata.stand_pose = choices[userdata.preference].pose except Exception as e: Logger.logwarn('Failed to retrieve pose information from service result:\n%s' % str(e)) self._failed = True return 'failed' self._done = True return 'done'
def on_enter(self, userdata): self._startTime = Time.now() self._failed = False self._reached = False goal_id = GoalID() goal_id.id = 'abcd' goal_id.stamp = Time.now() action_goal = MoveBaseGoal() action_goal.target_pose = userdata.waypoint action_goal.speed = userdata.speed if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: self._move_client.send_goal(self._action_topic, action_goal) resp = self.set_tolerance(goal_id, 0.2, 1.55) except Exception as e: Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True Logger.loginfo('Driving to next waypoint')
def set_push_distance(self, affordance): '''Sets the displacement of the "insert" affordance to a positive number.''' push_distance = +abs(self._push_distance) affordance.displacement = push_distance Logger.loginfo("Will be retracting by %d centimeters" % (push_distance*100)) return affordance
def set_cut_circle_angle(self, affordance): '''Sets the angle of the circular affordance for cutting a circle.''' cut_angle_degrees = math.copysign(self._cut_angle, affordance.displacement) affordance.displacement = math.radians(cut_angle_degrees) Logger.loginfo("Will be cutting by %d degrees" % cut_angle_degrees) return affordance
def set_retract_distance(self, affordance): '''Sets the displacement of the "insert" affordance to a negative number.''' retract_distance = -abs(self._insert_distance) # negative number means retract affordance.displacement = retract_distance Logger.loginfo("Will be retracting by %d centimeters" % (retract_distance*100)) return affordance
def print_offset_info(self, offsets): sides = ['left_arm', 'right_arm'] for side in sides: Logger.loginfo("Joint order (%s): %s" % (side, str(self._joint_names[side][0:4]))) rounded_offsets = [round(offset, 3) for offset in offsets[side]['avg']] # round due to comms_bridge Logger.loginfo("Offsets (%s): %s" % (side, str(rounded_offsets))) # Logger.loginfo("Max deviation from average (%s): %s" % (side, str(offsets[side]['diff']))) pprint.pprint(offsets) # Pretty print to the "onboard" terminal
def execute(self, userdata): answer = VictimAnswer() answer.task_id = userdata.task_details_task_id answer.answer = VictimAnswer.CONFIRM self._pub.publish(self._topicVictimReached, answer) Logger.loginfo("Victim Found, id = " + str(userdata.task_details_task_id)) return "succeeded"
def get_air_sump_pressure(self, input): '''Returns the current air sump pressure.''' robot_status = self._sub.get_last_msg(self._status_topic) air_pressure = robot_status.air_sump_pressure Logger.loginfo('Air Sump Pressure: %.2f' % air_pressure) return air_pressure
def on_enter(self, userdata): state = ObjectState() state.state = -2 request = SetObjectStateRequest(userdata.victim, state) resp = self._srvSet.call(self._setVictimState, request) Logger.loginfo('%(x)s discarded' % { 'x': userdata.victim }) return 'discarded'
def on_enter(self, userdata): """Upon entering the state""" bash_command = ["/bin/bash", "--norc", "-c"] # Start Bagging bag_command = "rosbag record -O {}".format(userdata.bagfile_name) + " " + self._topics_to_record.replace(",", "") self._bag_process = subprocess.Popen(bash_command + [bag_command], stdout=subprocess.PIPE, preexec_fn=os.setsid) userdata.rosbag_process = self._bag_process Logger.loginfo('Recording topics to %s' % userdata.bagfile_name)
def on_enter(self, userdata): task_local = self._sub.get_last_msg(self._allocated_task) if len(task_local.details.floatParams) != 4: pass else: userdata.task_details_task_id = task_local.details.task_id userdata.pose.pose = task_local.pose Logger.loginfo(str(task_local.details.floatParams[4])) userdata.params_distance = task_local.details.floatParams[SarTaskTypes.INDEX_VICTIM_DISTANCE]
def on_enter(self, userdata): goal = maryttsGoal() goal.text = userdata.text Logger.loginfo("Speech output, saying:\n%s" % goal.text) self._error = False try: self._client.send_goal(self._topic, goal) except Exception as e: Logger.logwarn("Failed to send the Sweep command:\n%s" % str(e)) self._error = True
def execute(self, userdata): if self._move_client.has_result(self._action_topic): result = self._move_client.get_result(self._action_topic) if result.result == 1: self._reached = True Logger.loginfo('Exploration succeeded') return 'succeeded' else: self._failed = True Logger.logwarn('Exploration failed!') 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. # The following code is just for illustrating how the behavior logger works. # Text logged by the behavior logger is sent to the operator and displayed in the GUI. time_to_wait = rospy.Time.now() - self._start_time - self._target_time if time_to_wait > 0: Logger.loginfo("Need to wait for %.1f seconds." % time_to_wait)
def on_enter(self, userdata): """Upon entering the state, get the first trajectory point and request moveit motion plan to get there.""" self._failed = False # Create the motion goal (one for both arms) move_group_goal = MoveGoal() move_group_goal.request = MotionPlanRequest() # Figure out which planning group is needed if len(userdata.trajectories) is 2: self._planning_group = "both_arms_group" else: chain = userdata.trajectories.keys()[0] if chain is 'left_arm': self._planning_group = "l_arm_group" elif chain is 'right_arm': self._planning_group = "r_arm_group" if chain is 'left_leg': self._planning_group = "l_leg_group" elif chain is 'right_leg': self._planning_group = "r_leg_group" else: Logger.logwarn('Unexpected key: %s Either "left" or "right" was expected.' % arm) move_group_goal.request.group_name = self._planning_group move_group_goal.request.max_velocity_scaling_factor = self._vel_scaling # Get first/starting trajectory point move_group_goal.request.goal_constraints = [Constraints()] move_group_goal.request.goal_constraints[0].joint_constraints = [] for arm, traj in userdata.trajectories.iteritems(): joint_names = traj.joint_names starting_point = traj.points[0].positions if len(joint_names) != len(starting_point): Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)" % (len(joint_names), len(starting_point))) for i in range(min(len(joint_names), len(starting_point))): move_group_goal.request.goal_constraints[0].joint_constraints.append(JointConstraint(joint_name = joint_names[i], position = starting_point[i]) ) # Send the motion plan request to the server try: Logger.loginfo("Moving %s to starting point." % self._planning_group) self._client.send_goal(self._action_topic, move_group_goal) except Exception as e: Logger.logwarn('Was unable to execute %s motion request:\n%s' % (self._planning_group, str(e))) self._failed = True
def __init__(self, planning_group, joint_names): """Constructor""" super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'], input_keys=['target_joint_config']) self._planning_group = planning_group self._joint_names = joint_names Logger.loginfo("About to make mgc in init with group %s" % self._planning_group) self.group_to_move = MoveGroupCommander(self._planning_group) Logger.loginfo("finished making mgc in init.") self._done = False
def on_enter(self, userdata): if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic) else: Logger.logwarn('Topic %s still not available, giving up.' % self._topic) if self._connected and self._clear and self._sub.has_msg(self._topic): self._sub.remove_last_msg(self._topic)
def __init__(self): ''' Constructor ''' super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed', 'update'], input_keys=['waypoint','victim','speed']) self._action_topic = '/move_base' Logger.loginfo("OUTPUT TEST") self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction}) self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance) self._failed = False self._reached = False
def on_exit(self, userdata): if not self._client.finished(): self._client.cancel() Logger.loginfo("Cancelled active action goal.")
def on_exit(self, userdata): # This method is called when an outcome is returned and another state gets active. # It can be used to stop possibly running processes started by on_enter. Logger.loginfo('leaving listening state') pass # Nothing to do in this example.
def on_enter(self, userdata): Logger.loginfo('Entering Set_Expression')
def execute(self, userdata): if type(self.objects_list) == dict: self.object_names = self.objects_list.keys() #self.self.objects_list[obj_nm] = self.objects_list.values() Logger.loginfo('Detection Result: %s' %(self.object_names)) for obj_nm in self.object_names: if type(self.objects_list[obj_nm]) == list: for obj_l in self.objects_list[obj_nm]: text_marker, pose_marker = self.init_marker() text_marker.text = obj_nm text_marker.pose.position.x = obj_l.position.x - 0.1 text_marker.pose.position.y = obj_l.position.y text_marker.pose.position.z = obj_l.position.z + 0.1 text_marker.color.r = min(abs(text_marker.pose.position.x / 2), 1.0) text_marker.color.g = min(abs(text_marker.pose.position.y / 2), 1.0) text_marker.color.b = min(abs(text_marker.pose.position.z / 2), 1.0) pose_marker.pose.position.x = obj_l.position.x pose_marker.pose.position.y = obj_l.position.y pose_marker.pose.position.z = obj_l.position.z pose_marker.color.r = min(abs(pose_marker.pose.position.x / 2), 1.0) pose_marker.color.g = min(abs(pose_marker.pose.position.y / 2), 1.0) pose_marker.color.b = min(abs(pose_marker.pose.position.z / 2), 1.0) self.id_count += 1 text_marker.id = self.id_count text_marker.ns = "marker"+str(self.id_count) self.marker_array_data.markers.append(text_marker) self.id_count += 1 pose_marker.id = self.id_count pose_marker.ns = "marker"+str(self.id_count) self.marker_array_data.markers.append(pose_marker) else: text_marker, pose_marker = self.init_marker() text_marker.text = obj_nm text_marker.pose.position.x = self.objects_list[obj_nm].position.x - 0.1 text_marker.pose.position.y = self.objects_list[obj_nm].position.y text_marker.pose.position.z = self.objects_list[obj_nm].position.z + 0.1 text_marker.color.r = min(abs(text_marker.pose.position.x / 2), 1.0) text_marker.color.g = min(abs(text_marker.pose.position.y / 2), 1.0) text_marker.color.b = min(abs(text_marker.pose.position.z / 2), 1.0) pose_marker.pose.position.x = self.objects_list[obj_nm].position.x pose_marker.pose.position.y = self.objects_list[obj_nm].position.y pose_marker.pose.position.z = self.objects_list[obj_nm].position.z pose_marker.color.r = min(abs(pose_marker.pose.position.x / 2), 1.0) pose_marker.color.g = min(abs(pose_marker.pose.position.y / 2), 1.0) pose_marker.color.b = min(abs(pose_marker.pose.position.z / 2), 1.0) self.id_count += 1 text_marker.id = self.id_count text_marker.ns = "marker"+str(self.id_count) self.marker_array_data.markers.append(text_marker) self.id_count += 1 text_marker.id = self.id_count pose_marker.ns = "marker"+str(self.id_count) self.marker_array_data.markers.append(pose_marker) elif type(self.objects_list) == Pose: for obj_ps in self.objects_list: pose_marker = Marker() pose_marker.type = 2 # sphere pose_marker.header.frame_id = "map" pose_marker.header.stamp = rospy.get_rostime() pose_marker.id = self.id_count pose_marker.action = Marker.ADD pose_marker.pose.position.x = obj_ps.position.x pose_marker.pose.position.y = obj_ps.position.y pose_marker.pose.position.z = obj_ps.position.z pose_marker.pose.orientation.x = 0.0 pose_marker.pose.orientation.y = 0.0 pose_marker.pose.orientation.z = 0.0 pose_marker.pose.orientation.w = 0.0 pose_marker.color.r = min(abs(pose_marker.pose.position.x / 2), 1.0) pose_marker.color.g = min(abs(pose_marker.pose.position.y / 2), 1.0) pose_marker.color.b = min(abs(pose_marker.pose.position.z / 2), 1.0) pose_marker.color.a = 0.8 pose_marker.scale.x = 0.07 pose_marker.scale.y = 0.07 pose_marker.scale.z = 0.07 pose_marker.lifetime = rospy.Duration(100) self.id_count += 1 pose_marker.ns = "marker"+str(self.id_count) self.marker_array_data.markers.append(pose_marker) #print self.marker_array_data rospy.sleep(0.1) self.pub_marker_array.publish(self.marker_array_data) rospy.sleep(0.1) self.pub_marker_array.publish(self.marker_array_data) return 'continue'
def on_stop(self): Logger.loginfo("Drive FWD STOPPED!")
def on_enter(self, userdata): Logger.loginfo('Enter Move Arm') self.timeout = rospy.Time.now() if type(userdata.targetPose) is Pose: Logger.loginfo('target is a pose') waypoints = [ self.group.get_current_pose().pose, copy.deepcopy(userdata.targetPose) ] try: (plan, fraction) = self.group.compute_cartesian_path( waypoints, self.movSteps, self.jumpThresh) except: Logger.loginfo('Planning failed; could not compute path') self.result = 'failed' return else: Logger.loginfo('ERROR in ' + str(self.name) + ' : target is not a Pose()') self.result = 'failed' Logger.loginfo('target defined') try: Logger.loginfo(str(plan)) self.endState = plan.joint_trajectory.points[ len(plan.joint_trajectory.points) - 1].positions Logger.loginfo(str(self.endState)) except: Logger.loginfo('Planning failed; path is invalid') self.result = 'failed' return Logger.loginfo('Plan done successfully') if self.move: Logger.loginfo('Initiating movement') self.group.execute(plan, wait=False) if not self.waitForExecution: self.result = "done" else: Logger.loginfo('The target is reachable') self.result = "done"
def on_start(self): Logger.loginfo('Drive FWD READY!')
def on_exit(self, userdata): self.cmd_pub.linear.x = 0.0 self.pub.publish(self.vel_topic, self.cmd_pub) Logger.loginfo('Drive FWD ENDED!')
def on_start(self): Logger.loginfo("Drive FWD READY!") self._start_time = rospy.Time.now() #bug detected! (move to on_enter)
def set_full_rotation_angle(self, affordance): degrees = math.copysign(self._turn_amount, affordance.displacement) affordance.displacement = math.radians(degrees) Logger.loginfo("Turning by %.1f degrees" % degrees) return affordance
def on_exit(self, userdata): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo('Cancelled active action goal.')
def on_enter(self, userdata): Logger.loginfo('Aligning robot') self.client.send_goal(self._topic, riptide_controllers.msg.GateManeuverGoal())
def on_exit(self, userdata): Logger.loginfo("exit started website_dummy_state1!")
def on_exit(self, userdata): self.cmd_pub.vel = 0.0 self.cmd_pub.angle = 0.0 self.pub.publish(self.vel_topic, self.cmd_pub) Logger.loginfo("Drive FWD ENDED!")
def on_start(self): Logger.loginfo("started website_dummy_state1 ready!")
def on_stop(self): #os.system("pgrep -f face_motor_server.py") #os.system("pkill -9 -f face_motor_server.py") Logger.loginfo('on_stop stopping the face motor server') pass # Nothing to do in this example.
def on_enter(self, userdata): Logger.loginfo('Pitching with angle %f' % userdata.angle) self.client.send_goal( self._topic, riptide_controllers.msg.GoToPitchGoal(userdata.angle))
def on_enter(self, userdata): Logger.loginfo("entering FacialExpressionState") print("making face now") os.system("python3 {}catkin_ws/src/robot_face/src/pick_facial_expression.py {}".format(home,str(self._expression_num))) pass
def execute(self, d): torso_state, people, faces = None, None, None ra = rospy.Rate(self.rate) # Logger.loginfo('checking for torso state and people') got_it = False # wait until we have data torso_state = self._subs.get_last_msg(self.TORSO_STATE_TOPIC) people = self._subs.get_last_msg(self.PEOPLE_TOPIC) if torso_state is None: Logger.loginfo('no torso state, please start torso once') return elif people is None: Logger.loginfo('no people') elif len(people.people) < 1: #Logger.loginfo('no people') pass else: got_it = True Logger.loginfo('Got initial people and torso state.') if not got_it: Logger.loginfo('Dit not get initial people and torso state.') #return cmd = None; # first, get new data if not None ts = self._subs.get_last_msg(self.TORSO_STATE_TOPIC) pe = self._subs.get_last_msg(self.PEOPLE_TOPIC) # store latest faces -> only if we got some, we adapt j1 if self.with_j1: faces = self._subs.get_last_msg(self.FACES_TOPIC) if faces is not None and rospy.Time.now() - faces.header.stamp > rospy.Duration(1): faces = None got_faces = faces is not None and faces.count > 0 if ts is not None: torso_state = ts if pe is not None: if len(pe.people) > 0: people = pe ps = PointStamped() ps.header = people.header ps.header.stamp = rospy.Time(0) ps.point = people.people[0].position trans = self.t.transformPoint(self.TARGET_FRAME, ps) person_pos = np.arctan2(trans.point.y, trans.point.x) person_dist = np.linalg.norm([trans.point.x,trans.point.y]) js_pos = torso_state.actual.positions js_pos_des = torso_state.desired.positions dur = [] traj = JointTrajectory() traj.joint_names = self.JOINT_NAMES point = JointTrajectoryPoint() pos = js_pos[0] cmd = np.clip(person_pos, self.LIMITS[self.JOINT_NAMES[0]][0], self.LIMITS[self.JOINT_NAMES[0]][1]) dur.append(max(abs(cmd - pos) / self.MAX_VEL, self.MIN_TRAJ_DUR)) if got_faces: ps = PointStamped() ps.header = faces.header ps.header.stamp = rospy.Time(0) ps.point = faces.faces[0].head_pose.position trans = self.t.transformPoint(self.TARGET_FRAME, ps) person_pos = np.arctan2(trans.point.y, trans.point.x) person_dist = np.linalg.norm([trans.point.x,trans.point.y]) js_pos = torso_state.actual.positions js_pos_des = torso_state.desired.positions dur = [] traj = JointTrajectory() traj.joint_names = self.JOINT_NAMES point = JointTrajectoryPoint() pos = js_pos[0] cmd = np.clip(person_pos, self.LIMITS[self.JOINT_NAMES[0]][0], self.LIMITS[self.JOINT_NAMES[0]][1]) dur.append(max(abs(cmd - pos) / self.MAX_VEL, self.MIN_TRAJ_DUR)) if cmd is None: return; # whether to adapt j1minimal state changes or not if self.with_j1 and not self.j1_done and got_faces: if faces is None: print('faces none') elif faces.count < 1: print('no faces') f_pos = faces.faces[0].head_pose ps = PointStamped() ps.header = faces.header ps.header.stamp = rospy.Time(0) ps.point = f_pos.position trans = self.t.transformPoint(self.TARGET_FRAME, ps) if trans is None: print('trans none') faces_pos = -np.arctan2(trans.point.z - 0.7, trans.point.x) / 2 j1cmd = np.clip(faces_pos, self.LIMITS[self.JOINT_NAMES[1]][0], self.LIMITS[self.JOINT_NAMES[1]][1]) j1pos = js_pos[1] dur.append(max(abs(j1cmd - j1pos) / self.MAX_VEL, self.MIN_TRAJ_DUR)) point.positions = [cmd, j1cmd] else: point.positions = [cmd, js_pos_des[1]] point.time_from_start = rospy.Duration(max(dur) / self.SPEED_SCALE) traj.points.append(point) # Logger.loginfo('new torso state: ' + str(point.positions)) self._pub.publish(self.TORSO_COMMAND_TOPIC, traj) if float(person_dist) < float(self.person_stop_dist): Logger.loginfo('Person {} nearer than {}. Stopping.'.format(person_dist, self.person_stop_dist)) return 'done'
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 on_enter(self, userdata): os.system("pgrep -f face_motor_server.py") os.system("pkill -9 -f face_motor_server.py") Logger.loginfo('stopping the face motor server')
def on_pause(self): Logger.loginfo('Pausing movement') self.group.stop()
def on_enter(self, userdata): Logger.loginfo('Rolling with angle %f' % self._angle) self.client.send_goal( self._topic, riptide_controllers.msg.GoToRollGoal(self._angle))
def create(self): bag_folder_in = "~/sysid_tests" bag_folder_out = "" # optional (if not provided, an /out folder will be created in the bag_folder_in directory prefix = "" # optional (applies to bag and video files) input_bagfiles = [] # calculated (leave empty) output_bagfiles = [] # calculated (leave empty) desired_num_tests = 1 # num of tests per trajectory wait_time = 3.0 # before execution (for rosbag record) settling_time = 2.0 # after execution mode_for_tests = "dance" # "manipulate", "system_id", etc. desired_controllers = ["left_arm_traj_controller","right_arm_traj_controller", "left_leg_traj_controller","right_leg_traj_controller","torso_traj_controller"] # x:880 y:104, x:660 y:14 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.traj_index = 0 _state_machine.userdata.tests_counter = 0 _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # Get folder containing input bagfiles and create folder for output bag_folder_in = os.path.expanduser(bag_folder_in) if not os.path.exists(bag_folder_in): Logger.logwarn('Path to input bag folder does not exist (%s)' % bag_folder_in) if not bag_folder_out: bag_folder_out = os.path.join(bag_folder_in, 'out') if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) else: # The user has provided a directory for output bagfiles bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Initialize lists again, in case the user did alter them input_bagfiles = [] output_bagfiles = [] # Get all input bagfile names from bag folder and also name the corresponding output bagfiles os.chdir(bag_folder_in) for bagfile in sorted(glob.glob("*.bag")): input_bagfiles.append(os.path.join(bag_folder_in, bagfile)) bare_name = bagfile.split(".")[0] output_name = bare_name + "_" + time.strftime("%Y-%m-%d-%H_%M") + "_" # file name will be completed by flexible calculation state output_bagfiles.append(output_name) Logger.loginfo('Found %d input bag files in %s' % (len(input_bagfiles), bag_folder_in)) # Create STAND posture trajectories _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory() # [/MANUAL_CREATE] # x:836 y:51, x:858 y:296 _sm_starting_point_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add('Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'starting_name'}) # x:42 y:231 OperatableStateMachine.add('Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting'}) # x:38 y:330 OperatableStateMachine.add('Wait_for_Rosbag_Record', WaitState(wait_time=wait_time), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add('Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'starting_name', 'output_value': 'output_bagfile_starting'}) # x:536 y:47 OperatableStateMachine.add('Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm'}) # x:236 y:655 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm'}) # x:272 y:47 OperatableStateMachine.add('Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=desired_controllers), transitions={'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add('Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add('Extract_Left_Arm_Part', CalculationState(calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_arm'}) # x:21 y:507 OperatableStateMachine.add('Extract_Right_Arm_Part', CalculationState(calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_arm'}) # x:293 y:146 OperatableStateMachine.add('Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=['left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso']), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all'}) # x:789 y:167 OperatableStateMachine.add('Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add('Extract_Left_Leg_Part', CalculationState(calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_leg'}) # x:22 y:665 OperatableStateMachine.add('Extract_Right_Leg_Part', CalculationState(calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_leg'}) # x:33 y:765 OperatableStateMachine.add('Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_torso'}) # x:227 y:410 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg'}) # x:237 y:531 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg'}) # x:241 y:296 OperatableStateMachine.add('Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso'}) # x:1118 y:103, x:348 y:32 _sm_execute_individual_trajectory_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['traj_index', 'tests_counter']) with _sm_execute_individual_trajectory_1: # x:79 y:28 OperatableStateMachine.add('Get_Input_Bagfile', CalculationState(calculation=lambda idx: input_bagfiles[idx]), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'traj_index', 'output_value': 'input_bagfile'}) # x:904 y:187 OperatableStateMachine.add('Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add('Wait_For_Rosbag_Record', WaitState(wait_time=wait_time), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add('Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process'}) # x:484 y:106 OperatableStateMachine.add('Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=settling_time), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add('Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=desired_controllers), transitions={'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories'}) # x:48 y:283 OperatableStateMachine.add('Load_Trajs_from_Bagfile', LoadTrajectoryFromBagfileState(), transitions={'done': 'Start_Video_Logging', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'bagfile_name': 'input_bagfile', 'trajectories': 'trajectories'}) # x:65 y:113 OperatableStateMachine.add('Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: prefix + output_bagfiles[i[0]] + str(i[1] + 1), input_keys=["idx", "counter"]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'idx': 'traj_index', 'counter': 'tests_counter', 'output_value': 'experiment_name'}) # x:56 y:192 OperatableStateMachine.add('Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Load_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'output_bagfile'}) # x:903 y:97 OperatableStateMachine.add('Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:472 y:26 OperatableStateMachine.add('Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'experiment_name': 'experiment_name', 'trajectories': 'trajectories'}) # x:71 y:384 OperatableStateMachine.add('Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:475 y:405 _sm_execute_sysid_trajectories_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['traj_index', 'tests_counter'], output_keys=['traj_index']) with _sm_execute_sysid_trajectories_2: # x:367 y:24 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={'finished': 'Increment_Tests_per_Traj_counter', 'failed': 'Wait_before_Fail'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'}) # x:714 y:27 OperatableStateMachine.add('Increment_Tests_per_Traj_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'More_Tests_for_this_Traj?'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'}) # x:738 y:207 OperatableStateMachine.add('More_Tests_for_this_Traj?', DecisionState(outcomes=['yes', 'no'], conditions=lambda counter: 'no' if counter >= desired_num_tests else 'yes'), transitions={'yes': 'Execute_Individual_Trajectory', 'no': 'Reset_Tests_counter'}, autonomy={'yes': Autonomy.Low, 'no': Autonomy.Low}, remapping={'input_value': 'tests_counter'}) # x:752 y:298 OperatableStateMachine.add('Reset_Tests_counter', CalculationState(calculation=lambda counter: 0), transitions={'done': 'Increment_Traj_Index'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'}) # x:752 y:399 OperatableStateMachine.add('Increment_Traj_Index', CalculationState(calculation=lambda idx: idx + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'traj_index', 'output_value': 'traj_index'}) # x:405 y:141 OperatableStateMachine.add('Wait_before_Fail', WaitState(wait_time=2.0), transitions={'done': 'Increment_Tests_per_Traj_counter'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:122 y:26 OperatableStateMachine.add('Starting_Execution', LogState(text="Execution is starting. Confirm first transition!", severity=Logger.REPORT_HINT), transitions={'done': 'Go_to_Desired_Mode'}, autonomy={'done': Autonomy.High}) # x:328 y:298 OperatableStateMachine.add('Execute SysID Trajectories', _sm_execute_sysid_trajectories_2, transitions={'finished': 'More_Trajectories?'}, autonomy={'finished': Autonomy.Inherit}, remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'}) # x:129 y:215 OperatableStateMachine.add('More_Trajectories?', DecisionState(outcomes=['yes', 'no'], conditions=lambda idx: 'no' if idx >= len(input_bagfiles) else 'yes'), transitions={'yes': 'Notify_Next_Trajectory', 'no': 'Move_to_Stand_Posture'}, autonomy={'yes': Autonomy.Low, 'no': Autonomy.High}, remapping={'input_value': 'traj_index'}) # x:592 y:103 OperatableStateMachine.add('Go_to_FREEZE_before_Exit', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.FREEZE), transitions={'changed': 'finished', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low}) # x:584 y:214 OperatableStateMachine.add('Failed_To_Go_To_Stand_Posture', LogState(text="Failed to go to stand posture", severity=Logger.REPORT_WARN), transitions={'done': 'Go_to_FREEZE_before_Exit'}, autonomy={'done': Autonomy.Off}) # x:121 y:393 OperatableStateMachine.add('Notify_Next_Trajectory', LogState(text="Continuing with next trajectory.", severity=Logger.REPORT_INFO), transitions={'done': 'Execute SysID Trajectories'}, autonomy={'done': Autonomy.Off}) # x:112 y:102 OperatableStateMachine.add('Go_to_Desired_Mode', ChangeControlModeActionState(target_mode=mode_for_tests), transitions={'changed': 'More_Trajectories?', 'failed': 'Go_to_FREEZE_before_Exit'}, autonomy={'changed': Autonomy.High, 'failed': Autonomy.High}) # x:329 y:158 OperatableStateMachine.add('Move_to_Stand_Posture', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_to_FREEZE_before_Exit', 'failed': 'Failed_To_Go_To_Stand_Posture'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.High}, remapping={'side': 'none'}) return _state_machine
def _behavior_execution(self, msg): # sending a behavior while one is already running is considered as switching 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...') # construct the behavior that should be executed 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 # perform the behavior switch if required with self._switch_lock: self._switching = True if self._running: self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start'])) # ensure that switching is possible 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 # wait if running behavior is currently starting or stopping rate = rospy.Rate(100) while not rospy.is_shutdown(): active_state = self.be.get_current_state() if active_state is not None or not self._running: break rate.sleep() # extract the active state if any if active_state is not None: 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._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed'])) return # stop the rest rospy.loginfo('Preempting current behavior version...') self.be.preempt_for_switch() # execute the behavior with self._run_lock: self._switching = False self.be = be self._running = True result = None 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, args=[str(result)])) 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)) import traceback Logger.loginfo(traceback.format_exc()) result = result or "exception" # only set result if not executed # done, remove left-overs like the temporary behavior file try: if not self._switching: self._clear_imports() self._cleanup_behavior(msg.behavior_checksum) except Exception as e: rospy.logerr('Failed to clean up behavior:\n%s' % str(e)) 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 self.be = None
def on_exit(self, userdata): if not self._client.has_result(self._motion_goal_ns): self._client.cancel(self._motion_goal_ns) Logger.loginfo("Cancelled active motion goal.")
def combine_plans(self, plans): output_dict = {} try: left_arm = plans[0] right_arm = plans[1] left_leg = plans[2] right_leg = plans[3] torso = plans[4] #hacky_dict_combination = lambda p: dict({'left_arm': p[0]}, **{'right_arm': p[1]}) # Logger.loginfo('SystemIdentificationTestsSM:combine_plans: plans="%s"' % str(plans)) if left_arm is not None: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Found left arm joint trajectory') output_dict['left_arm'] = left_arm.joint_trajectory if right_arm is not None: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Found right arm joint trajectory') output_dict['right_arm'] = right_arm.joint_trajectory if left_leg is not None: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Found left leg joint trajectory') output_dict['left_leg'] = left_leg.joint_trajectory if right_leg is not None: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Found right leg joint trajectory') output_dict['right_leg'] = right_leg.joint_trajectory if torso is not None: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Found torso joint trajectory') output_dict['torso'] = torso.joint_trajectory except Exception as e: Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Error "%s"' % (str(e))) return output_dict
def on_exit(self, userdata): Logger.loginfo('Stopping movement') self.group.stop()
def on_enter(self, userdata): Logger.loginfo('Moving %f m away from %s' % (2, userdata.args['obj'])) self.client.send_goal( self.topic, riptide_controllers.msg.GetDistanceGoal(userdata.args['obj']))
def cancel_active_goals(self): if self._client.is_available(self._action_topic): if self._client.is_active(self._action_topic): if not self._client.has_result(self._action_topic): self._client.cancel(self._action_topic) Logger.loginfo('Cancelled move_base active action goal.')