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