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 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):
'''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 __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_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._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):
"""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_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
def 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 combine_plans(self, plans):
output_dict = {}
try:
left_arm = plans[0]
right_arm = plans[1]
left_leg = plans[2]
right_leg = plans[3]
torso = plans[4]
#hacky_dict_combination = lambda p: dict({'left_arm': p[0]}, **{'right_arm': p[1]})
# Logger.loginfo('SystemIdentificationTestsSM:combine_plans: plans="%s"' % str(plans))
if left_arm is not None:
output_dict['left_arm'] = left_arm.joint_trajectory
if right_arm is not None:
output_dict['right_arm'] = right_arm.joint_trajectory
if left_leg is not None:
output_dict['left_leg'] = left_leg.joint_trajectory
if right_leg is not None:
output_dict['right_leg'] = right_leg.joint_trajectory
if torso is not None:
output_dict['torso'] = torso.joint_trajectory
except Exception as e:
Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Error "%s"' % (str(e)))
return output_dict
def on_enter(self, userdata):
# 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 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 set_back_rotation(self, input_keys):
affordance = input_keys[0]
back_rotation = input_keys[1]
degrees = math.copysign(back_rotation, affordance.displacement)
affordance.displacement = math.radians(degrees)
Logger.loginfo("Turning back %.1f degrees" % degrees)
return affordance
def __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 _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 __init__(self, config_name, move_group="", duration=1.0, wait_for_execution=True, action_topic='/execute_kinematic_path', robot_name=""):
'''
Constructor
'''
super(SrdfStateToMoveitExecute, self).__init__(
outcomes=['reached', 'request_failed', 'moveit_failed', 'param_error'])
self._config_name = config_name
self._robot_name = robot_name
self._move_group = move_group
self._duration = duration
self._wait_for_execution = wait_for_execution
self._action_topic = action_topic
self._client = ProxyServiceCaller({self._action_topic: ExecuteKnownTrajectory})
# self._action_topic = action_topic
# self._client = ProxyActionServer({self._action_topic: ExecuteTrajectoryAction})
self._request_failed = False
self._moveit_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
self._response = None
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 __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 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 __init__(self, config_name, move_group="", action_topic = '/move_group', robot_name=""):
'''
Constructor
'''
super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
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 execute(self, userdata):
# execute all active states
for name, state in self._states.items():
if self._returned_outcomes.has_key(name): continue
outcome = None
if isinstance(state, smach.StateMachine):
rmp_ud = smach.Remapper(
userdata,
state.get_registered_input_keys(),
state.get_registered_output_keys(),
{key: name + '_' + key for key in state.get_registered_input_keys() + state.get_registered_output_keys()})
state.userdata.merge(rmp_ud, rmp_ud.keys(), dict())
with state._state_transitioning_lock:
outcome = state._update_once()
else:
outcome = state.execute(smach.Remapper(
userdata,
state.get_registered_input_keys(),
state.get_registered_output_keys(),
{key: name + '_' + key for key in state.get_registered_input_keys() + state.get_registered_output_keys()}))
if outcome is not None and outcome != "loopback":
Logger.loginfo("Concurrency: %s > %s" % (name, outcome))
self._returned_outcomes[name] = outcome
# determine outcome
for element in self._outcome_mapping:
state_outcome = element['outcome']
mapping = element['condition']
if all(name in self._returned_outcomes and self._returned_outcomes[name] is outcome for name, outcome in mapping.items()):
return state_outcome
def 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 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 __init__(self):
self._sub = rospy.Subscriber("flexbe/request_behavior", BehaviorRequest, self._callback)
self._version_sub = rospy.Subscriber("flexbe/ui_version", String, self._version_callback)
self._pub = rospy.Publisher("flexbe/start_behavior", BehaviorSelection, queue_size=100)
self._status_pub = rospy.Publisher("flexbe/status", BEStatus, queue_size=100)
self._mirror_pub = rospy.Publisher("flexbe/mirror/structure", ContainerStructure, queue_size=100)
self._rp = RosPack()
self._behavior_lib = dict()
Logger.initialize()
behaviors_package = "flexbe_behaviors"
if rospy.has_param("behaviors_package"):
behaviors_package = rospy.get_param("behaviors_package")
else:
rospy.loginfo("Using default behaviors package: %s" % behaviors_package)
manifest_folder = os.path.join(self._rp.get_path(behaviors_package), 'behaviors/')
file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
for i in range(len(manifests)):
m = ET.parse(manifests[i]).getroot()
e = m.find("executable")
self._behavior_lib[i] = {
"name": m.get("name"),
"package": e.get("package_path").split(".")[0],
"file": e.get("package_path").split(".")[1],
"class": e.get("class")
}
rospy.loginfo("%d behaviors available, ready for start request." % len(self._behavior_lib.items()))
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 set_retract_distance(self, affordance):
'''Sets the displacement of the "insert" affordance to a negative number.'''
retract_distance = -abs(self._insert_distance) # negative number means retract
affordance.displacement = retract_distance
Logger.loginfo("Will be retracting by %d centimeters" % (retract_distance*100))
return affordance
def 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 set_push_distance(self, affordance):
'''Sets the displacement of the "insert" affordance to a positive number.'''
push_distance = +abs(self._push_distance)
affordance.displacement = push_distance
Logger.loginfo("Will be retracting by %d centimeters" % (push_distance*100))
return affordance
def execute(self, userdata):
if self._bumper_sub.has_msg(self._bumper_topic):
sensor = self._bumper_sub.get_last_msg(self._bumper_topic)
self._bumper_sub.remove_last_msg(self._bumper_topic)
if sensor.state > 0:
Logger.logwarn('Bumper %d contact' % (sensor.bumper))
self._return = 'bumper'
if self._cliff_sub.has_msg(self._cliff_topic):
sensor = self._cliff_sub.get_last_msg(self._cliff_topic)
self._cliff_sub.remove_last_msg(self._cliff_topic)
if sensor.state > 0:
Logger.logwarn('Cliff alert')
self._return = 'cliff'
return self._return
def check_plan(self, pose, orientation=False):
action_goal = make_default_action_goal(self, pose, orientation)
action_goal.planning_options.planning_scene_diff.is_diff = True
action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
action_goal.planning_options.plan_only = True
try:
self._client.send_goal(action_goal)
self._client.wait_for_result()
res = self._client.get_result()
print(res.error_code.val, self.planning_frame)
if res.error_code.val < 1:
return False
else:
return True
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
def on_enter(self, userdata):
self._failed = False
# create the motion goal
self._client.new_goal(self._planning_group)
self._client.add_joint_values(userdata.target_joint_config)
# execute the motion
try:
Logger.loginfo("Moving %s to: %s" %
(self._planning_group, ", ".join(
map(str, userdata.target_joint_config))))
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 execute(self, userdata):
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result:
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return 'done'
else:
Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string))
self._failed = True
return 'failed'
else:
Logger.logwarn('Wait for result returned True even though the result is %s' % str(result))
self._failed = True
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:
return 'failed'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
# Set output keys
userdata.outcome = {
'origin': 'get_path',
'outcome': result.outcome
}
userdata.path = result.path
# Send result log
Logger.loginfo(result.message)
# Based on the result, decide which outcome to trigger.
if result.outcome == GetPathResult.SUCCESS:
return 'succeeded'
elif result.outcome == GetPathResult.FAILURE:
return 'failed'
elif result.outcome == GetPathResult.NO_PATH_FOUND:
return 'failed'
elif result.outcome == GetPathResult.INVALID_GOAL:
return 'failed'
elif result.outcome == GetPathResult.INVALID_PLUGIN:
return 'aborted'
elif result.outcome == GetPathResult.INVALID_START:
return 'aborted'
elif result.outcome == GetPathResult.EMPTY_PATH:
return 'aborted'
elif result.outcome == GetPathResult.INTERNAL_ERROR:
return 'aborted'
elif result.outcome == GetPathResult.NOT_INITIALIZED:
return 'aborted'
elif result.outcome == GetPathResult.PAT_EXCEEDED:
return 'failed'
elif result.outcome == GetPathResult.STOPPED:
return 'aborted'
else:
return 'aborted'
def __init__(self):
'''
Constructor
'''
self.be = None
Logger.initialize()
smach.set_loggers(
rospy.logdebug, # hide SMACH transition log spamming
rospy.logwarn,
rospy.logdebug,
rospy.logerr)
#ProxyPublisher._simulate_delay = True
#ProxySubscriberCached._simulate_delay = True
# prepare temp folder
rp = rospkg.RosPack()
self._tmp_folder = tempfile.mkdtemp()
sys.path.append(self._tmp_folder)
rospy.on_shutdown(self._cleanup_tempdir)
# prepare manifest folder access
self._behavior_lib = BehaviorLibrary()
self._pub = ProxyPublisher()
self._sub = ProxySubscriberCached()
self.status_topic = 'flexbe/status'
self.feedback_topic = 'flexbe/command_feedback'
self._pub.createPublisher(self.status_topic, BEStatus, _latch=True)
self._pub.createPublisher(self.feedback_topic, CommandFeedback)
# listen for new behavior to start
self._running = False
self._switching = False
self._sub.subscribe('flexbe/start_behavior', BehaviorSelection,
self._behavior_callback)
# heartbeat
self._pub.createPublisher('flexbe/heartbeat', Empty)
self._execute_heartbeat()
rospy.sleep(0.5) # wait for publishers etc to really be set up
self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
def _convert_input_data(self, keys, values):
result = dict()
for k, v in zip(keys, values):
# action call has empty string as default, not a valid input key
if k == '':
continue
try:
result[k] = self._convert_dict(cast(v))
except ValueError:
# unquoted strings will raise a ValueError, so leave it as string in this case
result[k] = str(v)
except SyntaxError as se:
Logger.loginfo(
'Unable to parse input value for key "%s", assuming string:\n%s\n%s'
% (k, str(v), str(se)))
result[k] = str(v)
return result
def execute(self, userdata): '''Execute this state''' if self._subscriber.has_msg(self._topic): msg = self._subscriber.get_last_msg(self._topic) # in case you want to make sure the same message is not processed twice: self._subscriber.remove_last_msg(self._topic) if not msg.found: return 'success' self.calculate_error(msg) Logger.loghint(self._error_result) for key in self._keys: # check tolerance if self._tolerance < abs(self._error_result[key]): userdata.output_value = self._error_result return 'unacceptable'
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.
# Get transform between camera and robot1_base
while True:
rospy.sleep(0.1)
try:
target_pose = self._tf_buffer.transform(userdata.pose, "world")
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr(
"ComputeGraspState::on_enter - Failed to transform to world"
)
continue
# the grasp pose is defined as being located on top of the box
target_pose.pose.position.z += self._offset + 0.0
# rotate the object pose 180 degrees around - now works with -90???
#q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]
q_orig = [0, 0, 0, 1]
q_rot = quaternion_from_euler(self._rotation, 0, 0)
res_q = quaternion_multiply(q_rot, q_orig)
target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q)
# use ik service to compute joint_values
self._srv_req = GetPositionIKRequest()
self._srv_req.ik_request.group_name = self._group
self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(
self._group + '/joint_states', sensor_msgs.msg.JointState)
self._srv_req.ik_request.ik_link_name = self._tool_link # TODO: this needs to be a parameter
self._srv_req.ik_request.pose_stamped = target_pose
self._srv_req.ik_request.avoid_collisions = False
self._srv_req.ik_request.attempts = 500
try:
self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call IK: ' + str(e))
self._failed = True
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
request = GetTaskRequest()
# before userdata.task_details_task_id
request.task_id = 'victim_0_autonomous'
try:
response = self.getTask.call(self._topic_get_task, request).task
except Exception, e:
Logger.loginfo('[behavior_get_closer] Could not get task:' +
str(e))
rospy.sleep(0.5)
return 'waiting'
def on_enter(self, userdata):
# Update self.current_pose.
whole_body = self.robot.get('whole_body')
self.current_pose = whole_body.get_end_effector_pose(ref_frame_id='map')
target_poses = [poses for poses in userdata.target_poses.items()]
#target_poses = userdata.target_poses
category = userdata.category
# failed_objects = userdata.failed_objects
# collections = {"known": self.known_dict.keys(), "unknown": self.unknown_list}
candidates = [category]
print("category: ", candidates)
target_pose = [pose for pose in target_poses if pose[0] in candidates]
print("target: ", target_pose)
# target_pose = [pose for pose in target_pose if not pose[0] in [object for object, value in failed_objects.items() if value is True]]
if target_pose != []:
print('target_pose:', target_pose)
distance_array = np.array([])
for pose in target_pose:
distance = \
math.sqrt(pow((pose[1].position.x - self.current_pose.pos.x), 2) \
+ pow((pose[1].position.y - self.current_pose.pos.y), 2) \
+ pow((pose[1].position.z - self.current_pose.pos.z), 2))
distance_array = np.append(distance_array, distance)
distance_index = np.argsort(distance_array)
self.selected_pose_grasp = copy.deepcopy(target_pose[distance_index[0]][1])
self.selected_object_name = target_pose[distance_index[0]][0]
self.selected_object_status = 'Success!'
fix_orientation =self.quaternion_from_position(self.current_pose.pos, self.selected_pose_grasp.position)
self.selected_pose_grasp.orientation.x, self.selected_pose_grasp.orientation.y, \
self.selected_pose_grasp.orientation.z, self.selected_pose_grasp.orientation.w = fix_orientation
print("grasp: ", self.selected_pose_grasp)
print("name: ", self.selected_object_name)
# if self.check_plan(pose = self.selected_pose_grasp, orientation=True) == False:
# self.selected_pose_approach = None
# self.selected_pose_grasp = None
# self.selected_object_status = 'fail_plan'
print('object_name: ', self.selected_object_name)
print('object_status: ', self.selected_object_status)
else:
Logger.logwarn('detect result is empty')
self.selected_pose_grasp = None
self.selected_object_name = None
self.selected_object_status = 'not_detect'
def execute(self, userdata):
'''
Execute this state
'''
if self.return_code is not None:
# Return any previously assigned return code if we are in pause
userdata.status_text = self.status_text
return self.return_code
if self.client.has_result(self.current_action_topic):
result = self.client.get_result(self.current_action_topic)
if (result.valid):
self.return_code = 'valid'
self.status_text = 'checked robot state is valid'
else:
self.return_code = 'invalid'
self.status_text = 'checked robot state is not valid! number of contacts = %d' % (
len(result.contacts))
Logger.loginfo(self.status_text)
userdata.status_text = self.status_text
return self.return_code
elif self.client.get_state(
self.current_action_topic) == GoalStatus.ABORTED:
self.status_text = "StateValidation - %s request aborted by state validation action server\n %s" % (
self.name,
self.action_client.get_goal_status_text(
self.current_action_topic))
self.return_code = 'failed'
Logger.logerr(self.status_text)
userdata.status_text = self.status_text
return self.return_code
elif self.client.get_state(
self.current_action_topic) == GoalStatus.REJECTED:
# No result returned is returned for this action, so go by the client state
userdata.valid = self.valid
self.status_text = "StateValidation - %s request rejected by state validation action server" % (
self.name,
self.action_client.get_goal_status_text(
self.current_action_topic))
self.return_code = 'failed'
Logger.logerr(self.status_text)
userdata.status_text = self.status_text
return self.return_code
elif self.timeout_target is not None and rospy.Time.now(
) > self.timeout_target:
self.status_text = "StateValidation - timeout waiting for %s to state validation (%f > %f (%f))" % (
self.name, rospy.Time.now().to_sec(),
self.timeout_target.to_sec(), self.timeout_duration.to_sec())
self.return_code = 'failed'
Logger.logerr(self.status_text)
userdata.status_text = self.status_text
return self.return_code
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):
self.save_file = userdata.load_file
if self.tidy_order == []:
try:
dict2string_obj_inf = []
for k, v in userdata.detect_objects.items():
pose_txt = str(v.position.x)+","+ \
str(v.position.y)+","+ \
str(v.position.z)
feature = ""
for f in userdata.detect_objects_feature[k]:
feature += str(f) + ","
# rospy.loginfo("\n\n"+str(feature)+str(len(feature))+"\n\n")
dict2string_obj_inf.append(k)
dict2string_obj_inf.append(pose_txt)
dict2string_obj_inf.append(feature)
#print"SpaCoTyPlanning debug: "+str(dict2string_obj_inf)
req = self.em_spacoty_tidy_greedy(dict2string_obj_inf,
self.save_file)
except rospy.ServiceException, e:
rospy.loginfo("[Service spacoty/client] call failed: %s", e)
if req.done == True:
request_result = req.tidy_object_order_and_pose
self.low_obj_prob = req.low_obj_prob
#tidy_order = []
# string to dict
self.s2d = {}
for i in xrange(len(request_result)):
if i % 2 == 0:
p = Pose()
p_and_o = request_result[i + 1].split(",")
p.position.x = float(p_and_o[0])
p.position.y = float(p_and_o[1])
p.position.z = float(p_and_o[2])
#p.orientation.w = 1.0
self.s2d.update({request_result[i]: p})
self.tidy_order.append(request_result[i])
str_nm = "["
for i, nm in enumerate(self.tidy_order):
str_nm += " " + str(i) + ": " + nm
str_nm += "]"
Logger.loginfo("Plan: " + str_nm)
def __init__(self):
'''
Constructor
'''
super(GetJointNamesFromMoveGroupState,
self).__init__(outcomes=['retrieved', 'param_error'],
input_keys=['robot_name', 'selected_move_group'],
output_keys=['move_group', 'joint_names'])
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.return_code = None
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 = MoveBaseGoal()
goal.tolerance = self._tolerance
goal.target_pose = userdata.target_pose
goal.planner = self._planner
goal.controller = self._controller
goal.recovery_behaviors = self._recovery_behaviors
# 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:
Logger.logwarn('Failed to send goal:\n%s' % str(e))
self._error = True
def execute(self, userdata):
'''Execute this state'''
print("age: " +str(userdata.age))
print("gender: " +str(userdata.gender))
Logger.logwarn("age: " +str(userdata.age))
Logger.logwarn("gender: " +str(userdata.gender))
print("talking now")
if(userdata.age>40 and userdata.gender=="Male"):
os.system("python3 {}catkin_ws/src/robot_voice/src/ohbot_say_function.py {}".format(home,str(7)))
if(userdata.age<40 and userdata.gender=="Male"):
os.system("python3 {}catkin_ws/src/robot_voice/src/ohbot_say_function.py {}".format(home,str(8)))
if(userdata.gender=="Female"):
os.system("python3 {}catkin_ws/src/robot_voice/src/ohbot_say_function.py {}".format(home,str(9)))
return self._outcome
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
return 'arrived'
elif status in [GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED]:
Logger.logwarn('Navigation failed: %s' % str(status))
self._failed = True
return 'failed'
def calc_pose(self, input_list):
pose = input_list[0]
sdv = input_list[1]
out_pose = PoseStamped()
out_pose.pose.position.x = pose.pose.position.x + random.gauss(
self.waypoint_distance, sdv)
out_pose.pose.position.y = pose.pose.position.y + random.gauss(0, sdv)
out_pose.pose.orientation = Quaternion(w=1)
out_pose.header = Header(frame_id="world")
print out_pose
Logger.loginfo(
"Calculated new positon (%s/%s)." %
(str(out_pose.pose.position.x), str(out_pose.pose.position.y)))
return out_pose
def execute(self, userdata):
if self.result:
return self.result
if self.waitForExecution:
curState = self.group.get_current_joint_values()
diff = compareStates(curState, self.endState)
print("diff=" + str(diff))
if diff < self.tol or self.timeout + rospy.Duration(
self.watchdog) < rospy.Time.now():
self.count += 1
if self.count > 3:
Logger.loginfo('Target reached :)')
return "done"
else:
self.count = 0
else:
return "done"
def __init__(self, topic='/do_adaption'):
super(HandoverAdaptionReset,
self).__init__(outcomes=['succeeded', 'error'])
self._topic = topic
self._command = 0
self._reality_damp = 0.5
self._fixed_orientation = False
self._terminate = True
self._client = actionlib.SimpleActionClient(self._topic,
DoAdaptionAction)
Logger.loginfo('Waiting for adaption server ...')
self._client.wait_for_server()
Logger.loginfo('found adaption server!')
self._error = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.result == 1:
self._reached = True
return 'reached'
else:
self._failed = True
Logger.logwarn('Failed to reach waypoint!')
return 'failed'
def calc_rotation_angle(self, affordance):
'''Calculates the angle for the next planning step'''
if ( self._state_machine.userdata.move_to_poses ):
new_rotation_angle = math.radians(self.angle_increment_deg)
self.total_displacement += new_rotation_angle
else:
new_rotation_angle = affordance.displacement + math.radians(self.angle_increment_deg)
self.total_displacement = new_rotation_angle
self._state_machine.userdata.current_target_angle_deg = self._state_machine.userdata.current_target_angle_deg + self.angle_increment_deg
while ( self._state_machine.userdata.current_target_angle_deg >= 360 ):
self._state_machine.userdata.current_target_angle_deg = self._state_machine.userdata.current_target_angle_deg - 360;
affordance.displacement = new_rotation_angle
Logger.loginfo("Next rotation step: %f degrees (%f increment from current position)" % (self._state_machine.userdata.current_target_angle_deg, math.degrees(new_rotation_angle)))
return affordance
def on_exit(self, userdata):
self.cmd_pub.linear.x = 0.0
self.pub.publish(self.vel_topic, self.cmd_pub)
self.cmd_status = RobotStatus()
self.cmd_status.currentTask = ''
self.cmd_status.currentBehaviorStatus = ''
self.cmd_status.lastTask = ''
self.cmd_status.lastTaskResult = ''
self.cmd_status.nextTask = ''
self.cmd_status.allActions = []
self.cmd_status.indexCurrent = -1
self.cmd_status.actionResults = []
self.cmd_status.batteryCapacity = 0.0
self.cmd_status.batteryVoltage = 0.0
self.cmd_status.listen = False
Logger.loginfo("Drive FWD ENDED!")
def execute(self, userdata):
'''
Execute this state
'''
if self._param_error:
return 'param_error'
if self._request_failed:
return 'request_failed'
if self._response.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' %
str(self._response.error_code))
self._moveit_failed = True
return 'moveit_failed'
else:
Logger.loginfo('Move action succeeded: %s' %
str(self._response.error_code))
self._success = True
return 'reached'
def setGoal(self, pose):
rospy.wait_for_service('/move_base/clear_costmaps')
serv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
serv()
goal = MoveBaseGoal()
goal.target_pose.pose = pose
goal.target_pose.header.frame_id = "base_link"
# 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 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.
# 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('Failed to send the move command:\n%s' % str(e))
self._error = True
def execute(self, userdata):
if self._active:
"""Wait for action result and return outcome accordingly"""
self.Entity = None
# Get the entity's object in the latest detections
if self._sub.has_msg(self.entities_topic):
message = self._sub.get_last_msg(self.entities_topic)
for entity in message.entities:
if entity.ID == userdata.ID:
self.Entity = entity
print("entity found: "+str(entity.ID))
# If entity is defined, get the direction to it
if self.Entity:
if (self.Entity.name == "person"):
if (self.Entity.face.id != ""):
self.Entity.position.z += self.Entity.face.boundingBox.Center.z-0.1
else:
self.Entity.position.z += 1.6
ms = Float64()
serv = rospy.ServiceProxy('/get_direction', get_direction)
self.service.point = self.Entity.position
resp = serv(self.service)
# Publish to both Yaw and Pitch controllers
ms.data = min(max(-resp.pitch, -0), 1)
self.pubp.publish(ms)
ms.data = min(max(resp.yaw, -1.2), 1.2)
self.puby.publish(ms)
return
else:
Logger.logwarn("simulation mode: let's assume I'm looking at something")
return
ms = Float64()
ms.data = 0.3
self.pubp.publish(ms)
ms.data = 0
self.puby.publish(ms)
return "failed"
def on_start(self):
self._return = None # Clear completion flag
# Wait for odometry message
while (not self._odom_sub.has_msg(self._odom_topic)):
Logger.logwarn('Waiting for odometry message from the robot ')
rospy.sleep(0.05)
while (not self._odom_sub.has_msg(self._odom_topic)):
Logger.logwarn(
'Waiting for odometry message to become available from the robot '
)
rospy.sleep(0.25)
self._last_odom = self._sub.get_last_msg(self._odom_topic)
self._odom_frame = self._last_odom.header.frame_id
#Logger.loginfo(' odometry frame id <%s>' % (self._odom_frame))
# Update the target transformation
self._target.header.stamp = self._last_odom.header.stamp
while (self.transformOdom(self._target) is None):
Logger.logwarn(
'Waiting for tf transformations to odometry frame to become available from the robot '
)
rospy.sleep(0.25)
self._target.header.stamp = rospy.Time.now()
while (self.transformMap(self._last_odom) is None):
Logger.logwarn(
'Waiting for tf transformations to map frame become available from the robot '
)
rospy.sleep(0.25)
self._last_odom = self._sub.get_last_msg(self._odom_topic)
self._current_position = self.transformMap(self._last_odom)
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
if (self._marker_pub):
self._marker.action = Marker.ADD
self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible
self._marker.color.r = 0.0
self._marker.color.g = 0.0
self._marker.color.b = 1.0 # Indicate this target is planned
self._marker_pub.publish(self._marker_topic, self._marker)
self._marker_pub.publish(self._marker_topic,
self._reference_marker)
self._marker_pub.publish(self._marker_topic,
self._local_target_marker)
self._marker.action = Marker.MODIFY
self._reference_marker.action = Marker.MODIFY
self._reference_marker.color.a = 1.0 # Set alpha so it will become visible on next publish
self._local_target_marker.action = Marker.MODIFY
self._local_target_marker.color.a = 1.0 # Set alpha so it will become visible on next publish
def __init__(self,
timeout=5.0,
enter_wait_duration=0.0,
action_topic=None):
'''
Constructor
'''
super(JointValuesToMoveItPlanState,
self).__init__(outcomes=[
'planned', 'failed', 'topics_unavailable', 'param_error'
],
input_keys=[
'action_topic', 'move_group', 'joint_names',
'joint_values'
],
output_keys=['joint_trajectory'])
self.timeout_duration = rospy.Duration(timeout)
self.enter_wait_duration = enter_wait_duration
self.given_action_topic = action_topic
self.moveit_client = None
self.action_client = None
self.return_code = None
self.move_group = None
self.joint_names = None
self.joint_values = None
try:
self.moveit_client = ProxyMoveItClient(None)
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.moveit_client.setup_action_client(
self.given_action_topic, "MoveGroupAction",
self.enter_wait_duration)
else:
self.given_action_topic = None # override empty string
except Exception as e:
Logger.logerr(" Exception on initialization of %s\n %s" %
(self.name, str(e)))
self.current_action_topic = self.given_action_topic