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.')
