def on_enter(self, userdata):
"""On enter, create and send the follow joint trajectory action goal."""
self._failed = False
current_config = userdata.current_config
# Get the index of the joint whose value will be replaced
index_of_joint = current_config["joint_names"].index(self._joint_name)
# Replace the old joint value with the target_config's one
new_values = current_config["joint_values"]
new_values[index_of_joint] = self._joint_value
# Create trajectory point out of the revised joint values
point = JointTrajectoryPoint(positions=new_values, time_from_start=rospy.Duration.from_sec(self._time))
# Create trajectory message
trajectory = JointTrajectory(joint_names=current_config["joint_names"], points=[point])
action_goal = FollowJointTrajectoryGoal(trajectory=trajectory)
# execute the motion
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn("Failed to send trajectory action goal:\n%s" % str(e))
self._failed = True
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
if self._done:
return 'done'
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
self._done = True
return 'done'
elif result.error_code == FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED:
Logger.logwarn(
'Probably done, but goal tolerances violated (%d)' %
result.error_code)
self._done = True
return 'done'
else:
Logger.logwarn(
'Gripper trajectory failed to execute: (%d) %s' %
(result.error_code, result.error_string))
self._failed = True
return 'failed'
def execute(self, userdata):
if self.serviceNLU.is_available(self.serviceName):
try:
# Call the say service
srvRequest = ReceptionistNLUServiceRequest()
srvRequest.str = userdata.sentence
response = self.serviceNLU.call(self.serviceName, srvRequest)
if not response:
userdata.answer = "unknown"
return "fail"
# Checking the validity of the response
if response.response is "unknown":
userdata.answer = response.response
return "fail"
if response.response is "":
userdata.answer = "unknown"
return "fail"
userdata.answer = response.response
return "understood"
except rospy.ServiceException as exc:
Logger.logwarn("Receptionist NLU did not work: \n" + str(exc))
def __init__(self, config_name, srdf_file, move_group="", robot_name=""):
'''
Constructor
'''
super(GetJointsFromSrdfState,
self).__init__(outcomes=['retrieved', 'file_error'],
output_keys=['joint_values'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
# Check existence of SRDF file to reduce risk of runtime failure.
# Anyways, values will only be read during runtime to allow modifications.
self._srdf_path = None
try:
rp = RosPack()
pkg_name = srdf_file.split('/')[0]
self._srdf_path = os.path.join(rp.get_path(pkg_name),
'/'.join(srdf_file.split('/')[1:]))
if not os.path.isfile(self._srdf_path):
raise IOError('File "%s" does not exist!' % self._srdf_path)
except Exception as e:
Logger.logwarn('Unable to find given SRDF file: %s\n%s' %
(srdf_file, str(e)))
self._file_error = False
self._srdf = None
def execute(self, userdata):
if (self._sensor_sub.has_msg(self._sensor_topic)):
sensors = self._sub.get_last_msg(self._sensor_topic)
if ((sensors.bumps_wheeldrops > 0) or sensors.cliff_left or sensors.cliff_front_left or sensors.cliff_front_right or sensors.cliff_right):
Logger.logwarn('Bumper contact = %d cliff: left=%d %d %d %d = right ' %
(sensors.bumps_wheeldrops, sensors.cliff_left, sensors.cliff_front_left,sensors.cliff_front_right, sensors.cliff_right))
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'[MavrosSetMode]: ProxyServiceCaller: Topic \'{}\' not yet registered!'
.format(self._service_topic))
self._failed = True
return
try:
service_request = mavros_msgs.srv.SetModeRequest()
service_request.custom_mode = self._request
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.mode_sent:
self._failed = True
Logger.logwarn(
'[MavrosSetMode]: Calling \'{}\' was not successful: {}'.
format(self._service_topic, str(service_result.result)))
else:
Logger.loginfo(
'[MavrosSetMode]: Mavros mode set to: {}'.format(
self._request))
except Exception as e:
Logger.logerr(
'[MavrosSetMode]: Failed to call \'{}\' service request: {}'.
format(self._service_topic, str(e)))
self._failed = True
def __init__(self, config_name, srdf_file, move_group = "", robot_name = ""):
'''
Constructor
'''
super(GetJointsFromSrdfState, self).__init__(outcomes=['retrieved', 'file_error'],
output_keys=['joint_values'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
# Check existence of SRDF file to reduce risk of runtime failure.
# Anyways, values will only be read during runtime to allow modifications.
self._srdf_path = None
try:
rp = RosPack()
pkg_name = srdf_file.split('/')[0]
self._srdf_path = os.path.join(rp.get_path(pkg_name), '/'.join(srdf_file.split('/')[1:]))
if not os.path.isfile(self._srdf_path):
raise IOError('File "%s" does not exist!' % self._srdf_path)
except Exception as e:
Logger.logwarn('Unable to find given SRDF file: %s\n%s' % (srdf_file, str(e)))
self._file_error = False
self._srdf = None
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 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):
'''On enter, create and send the follow joint trajectory action goal.'''
self._failed = False
current_config = userdata.current_config
# Get the index of the joint whose value will be replaced
index_of_joint = current_config['joint_names'].index(self._joint_name)
# Replace the old joint value with the target_config's one
new_values = current_config['joint_values']
new_values[index_of_joint] = self._joint_value
# Create trajectory point out of the revised joint values
point = JointTrajectoryPoint(positions=new_values,
time_from_start=rospy.Duration.from_sec(
self._time))
# Create trajectory message
trajectory = JointTrajectory(joint_names=current_config['joint_names'],
points=[point])
action_goal = FollowJointTrajectoryGoal(trajectory=trajectory)
# execute the motion
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send trajectory action goal:\n%s' %
str(e))
self._failed = True
def on_enter(self, userdata):
"""Upon entering the state"""
try:
self._pub.publish(self._topic, OCSLogging(run=self._command, no_video=self._no_video, no_bags=self._no_bags, experiment_name=userdata.experiment_name, description=userdata.description))
except Exception as e:
Logger.logwarn('Could not send video logging command:\n %s' % str(e))
def on_enter(self, userdata):
'''Upon entering, create a neck trajectory and send the action goal.'''
self._failed = False
# Create neck point
neck_point = JointTrajectoryPoint()
neck_point.time_from_start = rospy.Duration.from_sec(1.0)
neck_point.positions.append(self._desired_tilt)
# Create neck trajectory
neck_trajectory = JointTrajectory()
neck_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
neck_trajectory.joint_names = ['neck_ry']
neck_trajectory.points.append(neck_point)
# Create action goal
action_goal = FollowJointTrajectoryGoal(trajectory = neck_trajectory)
# execute the motion
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send neck trajectory goal:\n%s' % str(e))
self._failed = True
def on_enter(self, userdata):
self._failed = False
self._done = False
# Create footstep planner request
strafing = self._direction == PatternParameters.STRAFE_LEFT or self._direction == PatternParameters.STRAFE_RIGHT
pattern_parameters = PatternParameters()
pattern_parameters.mode = self._direction
pattern_parameters.step_distance_forward = self._step_distance_forward if not strafing else 0.0 # will it ignore?
pattern_parameters.step_distance_sideward = self._step_distance_sideward if strafing else 0.0 # will it ignore?
pattern_parameters.close_step = True
step_distance = pattern_parameters.step_distance_sideward if strafing else pattern_parameters.step_distance_forward
pattern_parameters.steps = int(round(userdata.distance / step_distance))
request = StepPlanRequest()
request.parameter_set_name = String('drc_step_no_collision')
request.header = Header(frame_id = '/world', stamp = rospy.Time.now())
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.pattern_parameters = pattern_parameters
action_goal = StepPlanRequestGoal(plan_request = request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Was unable to create footstep pattern for wide stance:\n%s' % str(e))
self._failed = True
def on_enter(self, userdata):
"""Create and send action goal"""
self._done = False
self._failed = False
# Create and populate action goal
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self._joint_names
target_joint_positions = self._gripper_joint_positions[
self._gripper_cmd]
point = JointTrajectoryPoint()
point.positions = target_joint_positions
point.time_from_start = rospy.Duration.from_sec(self._time)
goal.trajectory.points.append(point)
# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn(
"Unable to send follow joint trajectory action goal:\n%s" %
str(e))
self._failed = True
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 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._planning_failed = False
self._control_failed = False
self._success = False
self._action_topic = userdata.move_group_prefix + userdata.action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = userdata.move_group
self._joint_names = None
self._joint_names = userdata.joint_names
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def on_pause(self):
try:
self.client.cancel(self.current_action_topic)
except Exception as e:
# client already closed
Logger.logwarn('Action client already closed - %s\n%s' %
(self.current_action_topic, str(e)))
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = FlyToGoal()
Logger.loginfo(
"[FlyToAction]: on enter goal x,y,z,yaw is %s,%s,%s,%s" %
(str(userdata.goal.x), str(userdata.goal.y), str(
userdata.goal.z), str(userdata.goal.yaw)))
#fill the goal
goal.points.append(userdata.goal)
goal.velocity = userdata.flying_velocity
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'[FlyToAction]: Failed to send the FlyToAction goal:\n%s' %
str(e))
self._error = True
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):
'''Upon entering the state, write trajectory goal(s) to action server(s)'''
self._failed = False
Logger.loginfo('Received trajectory for %s arm(s).' %
userdata.trajectories.keys())
Logger.loginfo('Using %s' % str(self._controllers))
# 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 arm, 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(1.0)
topic = self._client_topics[arm]
self._client.send_goal(topic, action_goal)
self._active_topics.append(topic)
Logger.loginfo('Goal message sent for %s trajectory' %
str(arm))
except Exception as e:
Logger.logwarn(
'Failed to send follow joint trajectory action goal(s):\n%s' %
str(e))
self._failed = True
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = self._joints
for i in range(0, len(self._joints)):
x = random.uniform(0, 1)
msg.position.append(self._minimal[i] * x + self._maximal[i] *
(1 - x))
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
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 on_enter(self, userdata):
speedValue = self._dynrec.get_configuration(timeout = 0.5)
if speedValue is not None:
self._defaultspeed = speedValue['speed']
self._dynrec.update_configuration({'speed':userdata.speed})
self._succeeded = False
self._failed = False
action_goal = MoveBaseGoal()
action_goal.exploration = True
action_goal.speed = userdata.speed
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
if self._move_client.is_active(self._action_topic):
self._move_client.cancel(self._action_topic)
self._move_client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to start Exploration' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
def __init__(self, controllers = ["left_arm_traj_controller", "right_arm_traj_controller"]):
'''Constructor'''
super(ExecuteTrajectoryBothArmsState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['trajectories'])
self._controllers = controllers
self._controllers = ["left_arm_traj_controller", "right_arm_traj_controller"] if not(controllers) else controllers
self._client = ProxyActionClient()
self._client_topics = dict()
self._active_topics = list()
for controller in self._controllers:
if "left_arm" in controller:
action_topic_left = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
self._client.setupClient(action_topic_left, FollowJointTrajectoryAction)
self._client_topics['left_arm'] = action_topic_left
elif "right_arm" in controller:
action_topic_right = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
self._client_topics['right_arm'] = action_topic_right
else:
Logger.logwarn('The controller is neither a left nor a right arm trajectory controller!? %s' % str(controller))
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
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 on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
#Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib['name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib['group'] #Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
return 'param_error'
try:
self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
return 'param_error'
#Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def on_enter(self, userdata):
self._failed = False
planning_group = ""
joint_values = []
if userdata.side == 'left' and self._target_pose in self._poses[self._robot]['left'].keys():
planning_group = self._poses[self._robot]['left'][self._target_pose]['group']
joint_values = self._poses[self._robot]['left'][self._target_pose]['joints']
elif userdata.side == 'right' and self._target_pose in self._poses[self._robot]['right'].keys():
planning_group = self._poses[self._robot]['right'][self._target_pose]['group']
joint_values = self._poses[self._robot]['right'][self._target_pose]['joints']
elif self._target_pose in self._poses[self._robot]['same'].keys():
planning_group = self._poses[self._robot]['same'][self._target_pose]['group']
joint_values = self._poses[self._robot]['same'][self._target_pose]['joints']
else:
Logger.logwarn('Specified target pose %d is not specified for %s side' % (self._target_pose, userdata.side))
self._failed = True
return
# create the motion goal
self._client.new_goal(planning_group)
self._client.add_joint_values(joint_values)
self._client.set_velocity_scaling(self._vel_scaling)
self._client.set_collision_avoidance(self._ignore_collisions)
self._client.add_link_padding(link_paddings = self._link_paddings) #dict
if self._is_cartesian:
self._client.set_cartesian_motion()
# execute the motion
try:
self._client.start_execution()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
self._failed = True
def on_enter(self, userdata):
"""Create and send action goal"""
self._arrived = False
self._failed = False
# Create and populate action goal
goal = MoveBaseGoal()
pt = Point(x=userdata.waypoint.x, y=userdata.waypoint.y)
qt = transformations.quaternion_from_euler(0, 0,
userdata.waypoint.theta)
goal.target_pose.pose = Pose(position=pt, orientation=Quaternion(*qt))
goal.target_pose.header.frame_id = "map"
# goal.target_pose.header.stamp.secs = 5.0
# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" %
str(e))
self._failed = True
def _version_callback(self, msg):
vui = self._parse_version(msg.data)
vex = self._parse_version(BehaviorLauncher.MIN_VERSION)
if vui < vex:
Logger.logwarn('FlexBE App needs to be updated!\n' \
+ 'Require at least version %s, but have %s\n' % (BehaviorLauncher.MIN_VERSION, msg.data) \
+ 'You can update the app by dropping the file flexbe_behavior_engine/FlexBE.crx onto the Chrome extension page.')
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
# initialize service proxy
if userdata.agv_id == 'agv1':
rospy.wait_for_service('/ariac/agv1')
NotifyShipmentReady = rospy.ServiceProxy('/ariac/agv1', AGVControl)
else:
if userdata.agv_id == 'agv2':
rospy.wait_for_service('/ariac/agv2')
NotifyShipmentReady = rospy.ServiceProxy(
'/ariac/agv2', AGVControl)
else:
userdata.success = None
userdata.message = None
return 'fail'
request = AGVControlRequest()
request.shipment_type = userdata.shipment_type
try:
srv_result = NotifyShipmentReady(request)
userdata.success = srv_result.success
userdata.message = srv_result.message
return 'continue'
except Exception as e:
Logger.logwarn('Could not submet shipment, service call failed')
rospy.logwarn(str(e))
userdata.message = None
return 'fail'
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._traj_exec_result:
# look at our cached 'execute_trajectory' result to see whether
# execution was successful
result = self._traj_exec_result
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn(
'Control failed for move action of group: %s (error code: %s)'
% (self._current_group_name, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn(
'Move action failed with result error code: %s' %
str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def execute(self, userdata):
if self._failed:
userdata.plan_header = None
return 'failed'
if self._done:
return 'planned'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.status.warning != ErrorStatus.NO_WARNING:
Logger.logwarn('Planning footsteps warning:\n%s' % result.status.warning_msg)
if result.status.error == ErrorStatus.NO_ERROR:
userdata.plan_header = result.step_plan.header
num_steps = len(result.step_plan.steps)
Logger.loginfo('Received plan with %d steps' % num_steps)
self._done = True
return 'planned'
else:
userdata.plan_header = None # as recommended: dont send out incomplete plan
Logger.logerr('Planning footsteps failed:\n%s' % result.status.error_msg)
self._failed = True
return 'failed'
def on_enter(self, userdata):
# create the motion goal
# Logger.loginfo("Entering MoveIt Commander code!")
# if len(self._joint_names) != len(userdata.target_joint_config):
# Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)"
# % (len(self._joint_names), len(userdata.target_joint_config)))
# self.group_to_move.set_joint_value_target(dict(zip(self._joint_names, userdata.target_joint_config)))
# execute the motion
try:
# KCLogger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
# result = self.group_to_move.go()
self._group.set_start_state_to_current_state()
self._group.clear_pose_targets()
self._group.get_current_pose()
self._group.set_named_target("reach_up")
result = self._group.go(wait=True)
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' %
str(e))
self._done = "failed"
if result:
self._done = "reached"
else:
self._done = "failed"
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):
Logger.logwarn("LOGGING THE MESSAGE I GOT FROM SUBSCRIBER: " +
str(userdata.input_value.data))
# userdata.message = userdata.input_value.data
'''Execute this state'''
return 'continue'
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
# Create the goal.
tweet = SendTweetGoal()
tweet.text = userdata.tweet_text
if len(userdata.tweet_text) > 140:
tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!'
tweet.with_photo = True
img = cv2.imread(userdata.picture_path)
bridge = CvBridge()
image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8")
tweet.photo = image_message
try:
self._client.send_goal(self._topic, tweet)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn('Failed to send the TweetPictureState command:\n%s' % str(e))
self._error = True
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):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position = []
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
if self._arrived:
return 'arrived'
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
if status == GoalStatus.SUCCEEDED:
self._arrived = True
Logger.loginfo('Arrived at (%.1f,%.1f) [%s]' %
(userdata.waypoint.pose.position.x,
userdata.waypoint.pose.position.y,
userdata.waypoint.header.frame_id))
return 'arrived'
elif status in [
GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED
]:
Logger.loginfo('Could not reach (%.1f,%.1f) [%s]' %
(userdata.waypoint.pose.position.x,
userdata.waypoint.pose.position.y,
userdata.waypoint.header.frame_id))
Logger.logwarn('Navigation failed: %s' % str(status))
self._failed = True
return 'failed'
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv.is_available(self._service_topic):
Logger.logerr(
'ProxyServiceCaller: Topic \'{}\' not yet registered!'.format(
self._service_topic))
self._failed = True
return
try:
service_request = ctop_planner.srv.GetExistingPlanCTOPRequest()
service_result = self._srv.call(self._service_topic,
service_request)
if not service_result.success:
self._failed = True
Logger.logwarn('Calling \'{}\' was not successful: {}'.format(
self._service_topic, str(service_result.message)))
else:
Logger.loginfo(service_result.message)
userdata.plans = service_result.plans
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(
self._service_topic, str(e)))
self._failed = True
def __init__(self, ref_frame, camera_topic, camera_frame):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DetectPartCameraState,
self).__init__(outcomes=['continue', 'failed'],
output_keys=['pose'])
self.ref_frame = ref_frame
self._topic = camera_topic
self._camera_frame = camera_frame
self._connected = False
self._failed = False
# tf to transfor the object pose
self._tf_buffer = tf2_ros.Buffer(
rospy.Duration(10.0)) #tf buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
# Subscribe to the topic for the logical camera
(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
else:
Logger.logwarn(
'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...'
% (self._topic, self.name, str(msg_topic)))
def on_enter(self, userdata):
self._failed = False
self._done = False
# Create footstep planner request
strafing = self._direction == PatternParameters.STRAFE_LEFT or self._direction == PatternParameters.STRAFE_RIGHT
pattern_parameters = PatternParameters()
pattern_parameters.mode = self._direction
pattern_parameters.step_distance_forward = self._step_distance_forward if not strafing else 0.0 # will it ignore?
pattern_parameters.step_distance_sideward = self._step_distance_sideward if strafing else 0.0 # will it ignore?
pattern_parameters.close_step = True
step_distance = pattern_parameters.step_distance_sideward if strafing else pattern_parameters.step_distance_forward
pattern_parameters.steps = int(round(userdata.distance /
step_distance))
request = StepPlanRequest()
request.parameter_set_name = String('drc_step_no_collision')
request.header = Header(frame_id='/world', stamp=rospy.Time.now())
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.pattern_parameters = pattern_parameters
action_goal = StepPlanRequestGoal(plan_request=request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn(
'Was unable to create footstep pattern for wide stance:\n%s' %
str(e))
self._failed = True
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self.ref_frame = userdata.ref_frame
self._topic = userdata.camera_topic
self._camera_frame = userdata.camera_frame
self._connected = False
self._failed = False
self._start_time = rospy.get_rostime()
# Subscribe to the topic for the logical camera
(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
else:
Logger.logwarn(
'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...'
% (self._topic, self.name, str(msg_topic)))
# Get transform between camera and robot_base
try:
self._transform = self._tf_buffer.lookup_transform(
self.ref_frame, self._camera_frame, rospy.Time(0),
rospy.Duration(1.0))
except Exception as e:
Logger.logwarn('Could not transform pose: ' + 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 on_enter(self, userdata):
self._failed = False
request = GetCartesianPathRequest()
request.group_name = self._move_group
request.avoid_collisions = not self._ignore_collisions
request.max_step = 0.05
request.header = userdata.eef_pose.header
request.waypoints.append(userdata.eef_pose.pose)
now = rospy.Time.now()
try:
self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1))
(p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now)
c = OrientationConstraint()
c.header.frame_id = 'base_link'
c.header.stamp = now
c.link_name = 'gripper_cam_link'
c.orientation.x = q[0]
c.orientation.y = q[1]
c.orientation.z = q[2]
c.orientation.w = q[3]
c.weight = 1
c.absolute_x_axis_tolerance = 0.1
c.absolute_y_axis_tolerance = 0.1
c.absolute_z_axis_tolerance = 0.1
#request.path_constraints.orientation_constraints.append(c)
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn('Exception while calculating path:\n%s' % str(e))
self._failed = True
def on_enter(self, userdata):
'''
Sending plan request with wide stance mode
'''
self._failed = False
self._done = False
pattern_parameters = PatternParameters()
pattern_parameters.steps = 4
pattern_parameters.mode = PatternParameters.FEET_REALIGN_ON_CENTER
pattern_parameters.close_step = True
pattern_parameters.extra_seperation = False
request = StepPlanRequest()
request.parameter_set_name = String('drc_step_no_collision')
request.header = Header(frame_id='/world', stamp=rospy.Time.now())
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.pattern_parameters = pattern_parameters
action_goal = StepPlanRequestGoal(plan_request=request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn(
'Was unable to create footstep pattern for wide stance:\n%s' %
str(e))
self._failed = True
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 _version_callback(self, msg):
vui = self._parse_version(msg.data)
vex = self._parse_version(BehaviorLauncher.MIN_VERSION)
if vui < vex:
Logger.logwarn('FlexBE App needs to be updated!\n' \
+ 'Require at least version %s, but have %s\n' % (BehaviorLauncher.MIN_VERSION, msg.data) \
+ 'Please run a "git pull" in "roscd flexbe_app".')
def on_enter(self, userdata):
'''
Sending plan request with wide stance mode
'''
self._failed = False
self._done = False
pattern_parameters = PatternParameters()
pattern_parameters.steps = 4
pattern_parameters.mode = PatternParameters.FEET_REALIGN_ON_CENTER
pattern_parameters.close_step = True
pattern_parameters.extra_seperation = False
request = StepPlanRequest()
request.parameter_set_name = String('drc_step_no_collision')
request.header = Header(frame_id = '/world', stamp = rospy.Time.now())
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.pattern_parameters = pattern_parameters
action_goal = StepPlanRequestGoal(plan_request = request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Was unable to create footstep pattern for wide stance:\n%s' % str(e))
self._failed = True
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
'''Upon entering the state, write trajectory goal(s) to action server(s)'''
self._failed = False
Logger.loginfo('Received trajectory for %s arm(s).' % userdata.trajectories.keys())
Logger.loginfo('Using %s' % str(self._controllers))
# 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 arm, 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(1.0)
topic = self._client_topics[arm]
self._client.send_goal(topic, action_goal)
self._active_topics.append(topic)
Logger.loginfo('Goal message sent for %s trajectory' % str(arm))
except Exception as e:
Logger.logwarn('Failed to send follow joint trajectory action goal(s):\n%s' % str(e))
self._failed = True
def on_enter(self, userdata):
rospy.loginfo(userdata.joint_names)
rospy.loginfo(userdata.joint_values)
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_names = userdata.joint_names
self._joint_values = userdata.joint_values
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(
joint_name=self._joint_names[i],
position=self._joint_values[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
#userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def on_enter(self, userdata):
try:
self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, 0)) # We don't care about the hand side
except Exception as e:
Logger.logwarn('Failed to send service call:\n%s' % str(e))
self._failed = True
def on_enter(self, userdata):
if self._calculation is not None:
try:
self._calculation_result = self._calculation(userdata.input_value)
except Exception as e:
Logger.logwarn('Failed to execute calculation function!\n%s' % str(e))
else:
Logger.logwarn('Passed no calculation!')
def execute(self, userdata):
if self._failed:
return "failed"
if self._result.error_code.val == MoveItErrorCodes.SUCCESS:
return "done"
else:
Logger.logwarn("Failed to execute trajectory: %d" % self._result.error_code.val)
return "failed"
def on_enter(self, userdata):
action_goal = GetLocomotionTargetPoseGoal()
action_goal.id_string = self._id_string
try:
self._client.send_goal(action_goal)
except Exception as e:
Logger.logwarn("Was unable to create pose perception request:\n%s" % str(e))
self._failed = True
def execute(self, userdata):
if rospy.Time.now() > self._start_waiting_time + self._wait_timeout:
userdata.person_pose = None
Logger.logwarn('Did not detect any person within %.1f seconds.' % self._wait_timeout)
return 'not_detected'
if self._sub.has_msg(self._topic):
userdata.person_pose = self._sub.get_last_msg(self._topic)
return 'detected'
