本文整理汇总了Python中World.World.get_nearest_object方法的典型用法代码示例。如果您正苦于以下问题：Python World.get_nearest_object方法的具体用法？Python World.get_nearest_object怎么用？Python World.get_nearest_object使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类World.World的用法示例。
在下文中一共展示了World.get_nearest_object方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __init__
# 需要导入模块: from World import World [as 别名]
# 或者: from World.World import get_nearest_object [as 别名]
#.........这里部分代码省略.........
else:
step = action.get_step(0)
if arm_index == 0:
self.arms.start_move_to_pose(step.armTarget.rArm, 0)
self.arms.set_gripper_state(0, step.gripperAction.rGripper)
else:
self.arms.start_move_to_pose(step.armTarget.lArm, 1)
self.arms.set_gripper_state(1, step.gripperAction.lGripper)
rospy.loginfo('Moved arm ' + str(arm_index) + ' to pose ' + pose_name)
def save_arm_pose(self, dummy=None):
'''Saves current arm state as an action step'''
states = self._get_arm_states()
step = ActionStep()
step.type = ActionStep.ARM_TARGET
step.armTarget = ArmTarget(states[0], states[1], 0.2, 0.2)
step.gripperAction = GripperAction(
self.arms.get_gripper_state(0),
self.arms.get_gripper_state(1))
self.session.save_arm_pose(step, self.world.get_frame_list())
return [RobotSpeech.STEP_RECORDED, GazeGoal.NOD]
def _get_arm_states(self):
'''Returns the current arms states in the right format'''
abs_ee_poses = [Arms.get_ee_state(0),
Arms.get_ee_state(1)]
joint_poses = [Arms.get_joint_state(0),
Arms.get_joint_state(1)]
states = [None, None]
for arm_index in [0, 1]:
nearest_obj = self.world.get_nearest_object(
abs_ee_poses[arm_index])
if (nearest_obj == None):
states[arm_index] = ArmState(ArmState.ROBOT_BASE,
abs_ee_poses[arm_index],
joint_poses[arm_index], Object())
else:
# Relative
rel_ee_pose = World.transform(
abs_ee_poses[arm_index],
'base_link', nearest_obj.name)
states[arm_index] = ArmState(ArmState.OBJECT,
rel_ee_pose,
joint_poses[arm_index], nearest_obj)
return states
def execute_action(self, dummy=None):
'''Starts the execution of the current action'''
if (self.session.n_actions() > 0):
if (self.session.n_frames() > 0):
self.session.save_current_action()
action = self.session.get_current_action()
if (action.is_object_required()):
if (self.world.update_object_pose()):
self.session.get_current_action().update_objects(
self.world.get_frame_list())
self.arms.start_execution(action)
else:
return [RobotSpeech.OBJECT_NOT_DETECTED,
GazeGoal.SHAKE]
else:示例2: __init__
# 需要导入模块: from World import World [as 别名]
# 或者: from World.World import get_nearest_object [as 别名]
#.........这里部分代码省略.........
if action is None:
rospy.logwarn('Arm pose does not exist:' + pose_name)
else:
step = action.get_step(0)
if arm_index == 0:
self.arms.start_move_to_pose(step.armTarget.rArm, 0)
if wait:
self._wait_for_arms()
self.arms.set_gripper_state(0, step.gripperAction.rGripper, wait=wait)
else:
self.arms.start_move_to_pose(step.armTarget.lArm, 1)
if wait:
self._wait_for_arms()
self.arms.set_gripper_state(1, step.gripperAction.lGripper, wait=wait)
rospy.loginfo('Moved arm ' + str(arm_index) + ' to pose ' + pose_name)
def _wait_for_arms(self):
rospy.loginfo('Will wait until the arms get in place.')
while (self.arms.is_executing()):
time.sleep(0.1)
rospy.loginfo('Arms are in place.')
def _get_arm_states(self):
'''Returns the current arms states in the right format'''
abs_ee_poses = [Arms.get_ee_state(0),
Arms.get_ee_state(1)]
joint_poses = [Arms.get_joint_state(0),
Arms.get_joint_state(1)]
states = [None, None]
for arm_index in [0, 1]:
nearest_obj = self.world.get_nearest_object(
abs_ee_poses[arm_index])
if (nearest_obj == None):
states[arm_index] = ArmState(ArmState.ROBOT_BASE,
abs_ee_poses[arm_index],
joint_poses[arm_index], Object())
else:
# Relative
rel_ee_pose = World.transform(
abs_ee_poses[arm_index],
'base_link', nearest_obj.name)
states[arm_index] = ArmState(ArmState.OBJECT,
rel_ee_pose,
joint_poses[arm_index], nearest_obj)
return states
def speech_command_cb(self, command):
'''Callback for when a speech command is received'''
if command.command in self.responses.keys():
rospy.loginfo('\033[32m Calling response for command ' +
command.command + '\033[0m')
response = self.responses[command.command]
if (not self.arms.is_executing() and not self._is_busy):
response.respond()
else:
if command.command == Command.STOP_EXECUTION:
response.respond()
else:
rospy.logwarn('Ignoring speech command during execution or busy: '
+ command.command)
else: