class ReadyArmActionServer: def __init__(self): self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.ready_arm_server = SimpleActionServer(ACTION_NAME, ReadyArmAction, execute_cb=self.ready_arm, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME) self.ready_arm_server.start() def ready_arm(self, goal): if self.ready_arm_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.move_arm_client.cancel_goal() self.ready_arm_server.set_preempted() if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, READY_POSITION, collision_operations=goal.collision_operations): self.ready_arm_server.set_succeeded() else: rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME) self.ready_arm_server.set_aborted()
class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while(not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
class axGripperServer: def __init__(self, name): self.fullname = name self.goalAngle = 0.0 self.actualAngle = 0.0 self.failureState = False dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5]) self.server = SimpleActionServer( self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False ) self.server.start() def execute_cb(self, goal): rospy.loginfo(goal) self.currentAngle = goal.command.position dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5]) attempts = 0 for i in range(10): rospy.sleep(0.1) attempts += 1 # while ... todo: add some condition to check on the actual angle # rospy.sleep(0.1) # print jPositions[4] # attempts += 1 if attempts < 20: self.server.set_succeeded() else: self.server.set_aborted() def checkFailureState(self): if self.failureState: print "I am currently in a failure state."
class axGripperServer: def __init__(self, name): self.fullname = name self.currentAngle = 0.0 # dynamixelChain.move_angle(7, 0.0, 0.5) dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5]) self.server = SimpleActionServer(self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False) self.server.start() def execute_cb(self, goal): rospy.loginfo(goal) self.currentAngle = goal.command.position dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5]) # dynamixelChain.move_angle(7, 0.1, 0.5) rospy.sleep(0.1) print jPositions[4] attempts = 0 while abs(goal.command.position - jPositions[4]) > 0.1 and attempts < 20: rospy.sleep(0.1) print jPositions[4] attempts += 1 if attempts < 20: self.server.set_succeeded() else: self.server.set_aborted()
class MockActionServer(object): """ MockActionServer base class """ def __init__(self, name, topic, action_type): """ Creating a custom mock action server.""" self._topic = topic self._name = name self._action_type = action_type self.timeout = 5 self.action_result = None Subscriber('mock/' + name, String, self.receive_commands) Subscriber('mock/gui_result', Bool, self.set_gui_result) self._server = ActionServer(self._topic, self._action_type, self.success, False) self._server.start() loginfo('>>> Starting ' + self._name) def receive_commands(self, msg): """ Decides the result of the next call. """ callback, timeout = msg.data.split(':') self.timeout = float(timeout) self._server.execute_callback = getattr(self, callback) logwarn('>>> ' + self._name + ': Current callback -> ' + callback) sleep(1) def abort(self, goal): """ Aborts any incoming goal. """ logwarn('>>> ' + self._name + ': This goal will be aborted.') sleep(self.timeout) self._server.set_aborted() def success(self, goal): """ Succeeds any incoming goal. """ logwarn('>>> ' + self._name + ': This goal will succeed.') sleep(self.timeout) self._server.set_succeeded(self.action_result) def preempt(self, goal): """ Preempts any incoming goal. """ logwarn('>>> ' + self._name + ': This goal will be preempted.') sleep(self.timeout) self._server.set_preempted() def set_gui_result(self, msg): """ Sets the result of the goal. """ self.action_result = ValidateVictimGUIResult() logwarn('>>> The gui response will be: ' + str(msg.data)) self.action_result.victimValid = msg.data
class MockExplorer(): def __init__(self, exploration_topic): self.robot_pose_ = PoseStamped() self.listener = tf.TransformListener() self.navigation_succedes = True self.reply = False self.preempted = 0 self.entered_exploration = False self.do_exploration_as_ = SimpleActionServer( exploration_topic, DoExplorationAction, execute_cb=self.do_exploration_cb, auto_start=False) self.do_exploration_as_.start() def __del__(self): self.do_exploration_as_.__del__() def do_exploration_cb(self, goal): rospy.loginfo('do_exploration_cb') self.entered_exploration = True while not self.reply: rospy.sleep(0.2) (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.robot_pose_.pose.position.x = trans[0] self.robot_pose_.pose.position.y = trans[1] feedback = DoExplorationFeedback() feedback.base_position.pose.position.x = \ self.robot_pose_.pose.position.x feedback.base_position.pose.position.y = \ self.robot_pose_.pose.position.y self.do_exploration_as_.publish_feedback(feedback) if self.do_exploration_as_.is_preempt_requested(): self.preempted += 1 rospy.loginfo("Preempted!") self.entered_exploration = False self.do_exploration_as_.set_preempted(DoExplorationResult()) return None else: result = DoExplorationResult() self.reply = False self.preempted = 0 self.entered_exploration = False if self.navigation_succedes: self.do_exploration_as_.set_succeded(result) else: self.do_exploration_as_.set_aborted(result)
class TestServer: def __init__(self,name): self._sas = SimpleActionServer(name, TestAction, execute_cb=self.execute_cb) def execute_cb(self, msg): if msg.goal == 0: self._sas.set_succeeded() elif msg.goal == 1: self._sas.set_aborted()
class MockActionServer(object): """ MockActionServer base class """ def __init__(self, name, topic, action_type): """ Creating a custom mock action server.""" self._topic = topic self._name = name self._action_type = action_type self.timeout = 1 self.action_result = None self.prefered_callback = ' ' Subscriber('mock/' + name, String, self.receive_commands) Subscriber('mock/gui_result', Bool, self.set_gui_result) self._server = ActionServer(self._topic, self._action_type, self.decide, False) self._server.start() loginfo('>>> Starting ' + self._name) def receive_commands(self, msg): """ Decides the result of the next call. """ callback, timeout = msg.data.split(':') # self.timeout = float(timeout) self.prefered_callback = callback sleep(1) def decide(self, goal): """ Deciding outcome of goal """ logwarn('Deciding callback...') sleep(self.timeout) if(self.prefered_callback == 'success'): logwarn('>>> ' + self._name + ': This goal will succeed.') self._server.set_succeeded() elif(self.prefered_callback == 'abort'): logwarn('>>> ' + self._name + ': This goal will be aborted.') self._server.set_aborted() elif(self.prefered_callback == 'preempt'): logwarn('>>> ' + self._name + ': This goal will be preempted.') self._server.set_preempted() else: logwarn('wtf?') def set_gui_result(self, msg): """ Sets the result of the goal. """ self.action_result = ValidateVictimGUIResult() logwarn('>>> The gui response will be: ' + str(msg.data)) self.action_result.victimValid = msg.data
class AveragingSVR2(object): def __init__(self): self._action = SimpleActionServer('averaging', AveragingAction, auto_start=False) self._action.register_preempt_callback(self.preempt_cb) self._action.register_goal_callback(self.goal_cb) self.reset_numbers() rospy.Subscriber('number', Float32, self.execute_loop) self._action.start() def std_dev(self, lst): ave = sum(lst) / len(lst) return sum([x * x for x in lst]) / len(lst) - ave**2 def goal_cb(self): self._goal = self._action.accept_new_goal() rospy.loginfo('goal callback %s' % (self._goal)) def preempt_cb(self): rospy.loginfo('preempt callback') self.reset_numbers() self._action.set_preempted(text='message for preempt') def reset_numbers(self): self._samples = [] def execute_loop(self, msg): if (not self._action.is_active()): return self._samples.append(msg.data) feedback = AveragingAction().action_feedback.feedback feedback.sample = len(self._samples) feedback.data = msg.data feedback.mean = sum(self._samples) / len(self._samples) feedback.std_dev = self.std_dev(self._samples) self._action.publish_feedback(feedback) ## sending result if (len(self._samples) >= self._goal.samples): result = AveragingAction().action_result.result result.mean = sum(self._samples) / len(self._samples) result.std_dev = self.std_dev(self._samples) rospy.loginfo('result: %s' % (result)) self.reset_numbers() if (result.mean > 0.5): self._action.set_succeeded(result=result, text='message for succeeded') else: self._action.set_aborted(result=result, text='message for aborted')
class ybGripperServer: def __init__(self, name): self.fullname = name self.currentValue = 0.0 gripperTopic = '/arm_1/gripper_controller/position_command' self.jppub = rospy.Publisher(gripperTopic, JointPositions) self.server = SimpleActionServer(self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False) self.server.start() def execute_cb(self, goal): rospy.loginfo(goal) self.currentValue = goal.command.position self.moveGripper(self.jppub, self.currentValue) attempts = 0 # here we should be checking if the gripper has gotten to its goal for i in range(5): rospy.sleep(0.1) attempts += 1 if attempts < 20: self.server.set_succeeded() else: self.server.set_aborted() def moveGripper(self, gPublisher, floatVal): jp = JointPositions() myPoison = Poison() myPoison.originator = 'yb_grip' myPoison.description = 'whoknows' myPoison.qos = 0.0 jp.poisonStamp = myPoison nowTime = rospy.Time.now() jvl = JointValue() jvl.timeStamp = nowTime jvl.joint_uri = 'gripper_finger_joint_l' jvl.unit = 'm' jvl.value = floatVal jp.positions.append(jvl) jvr = JointValue() jvr.timeStamp = nowTime jvr.joint_uri = 'gripper_finger_joint_r' jvr.unit = 'm' jvr.value = floatVal jp.positions.append(jvr) gPublisher.publish(jp) def ybspin(self): rospy.sleep(0.1)
class MockEndEffectorPlanner(): def __init__(self, end_effector_planner_topic): self.moves_end_effector = False self.move_end_effector_succeeded = False self.reply = False self.command = 0 self.point_of_interest = "" self.center_point = "" self.end_effector_planner_as_ = SimpleActionServer( end_effector_planner_topic, MoveEndEffectorAction, execute_cb=self.end_effector_planner_cb, auto_start=False) self.end_effector_planner_as_.start() def __del__(self): self.end_effector_planner_as_.__del__() def end_effector_planner_cb(self, goal): rospy.loginfo('end_effector_planner_cb') self.moves_end_effector = True self.command = goal.command self.point_of_interest = goal.point_of_interest self.center_point = goal.center_point while not self.reply: rospy.sleep(1.) if self.end_effector_planner_as_.is_preempt_requested(): self.end_effector_planner_as_.set_preempted(MoveEndEffectorResult()) self.moves_end_effector = False return None else: self.reply = False result = MoveEndEffectorResult() if self.move_end_effector_succeeded: self.moves_end_effector = False self.end_effector_planner_as_.set_succeeded(result) else: self.moves_end_effector = False self.end_effector_planner_as_.set_aborted(result)
class AveragingSVR(object): def __init__(self): self._action = SimpleActionServer('averaging', AveragingAction, execute_cb=self.execute_cb, auto_start=False) self._action.register_preempt_callback(self.preempt_cb) self._action.start() def std_dev(self, lst): ave = sum(lst) / len(lst) return sum([x * x for x in lst]) / len(lst) - ave**2 def preempt_cb(self): rospy.loginfo('preempt callback') self._action.set_preempted(text='message for preempt') def execute_cb(self, goal): rospy.loginfo('execute callback: %s' % (goal)) feedback = AveragingAction().action_feedback.feedback result = AveragingAction().action_result.result ## execute loop rate = rospy.Rate(1 / (0.01 + 0.99 * random.random())) samples = [] for i in range(goal.samples): sample = random.random() samples.append(sample) feedback.sample = i feedback.data = sample feedback.mean = sum(samples) / len(samples) feedback.std_dev = self.std_dev(samples) self._action.publish_feedback(feedback) rate.sleep() if (not self._action.is_active()): rospy.loginfo('not active') return ## sending result result.mean = sum(samples) / len(samples) result.std_dev = self.std_dev(samples) rospy.loginfo('result: %s' % (result)) if (result.mean > 0.5): self._action.set_succeeded(result=result, text='message for succeeded') else: self._action.set_aborted(result=result, text='message for aborted')
class MoveitMock(): """Mock for MoveIt which allows us to simulate errors during command execution.""" def __init__(self): rospy.logdebug("Ctor of MoveitMock called.") self._as_move_group = SimpleActionServer( 'move_group', MoveGroupAction, execute_cb=self.move_group_execute_callback, auto_start=False) self._as_sequence_group = SimpleActionServer( 'sequence_move_group', MoveGroupSequenceAction, execute_cb=self.blend_move_group_execute_callback, auto_start=False) self._as_move_group.start() self._as_sequence_group.start() # Service to signal test that node is running. self.status_service = rospy.Service('moveit_mock_status', Empty, self.handle_status_service) rospy.loginfo('MoveitMock started') def handle_status_service(self, req): pass def move_group_execute_callback(self, goal): rospy.loginfo("MoveGroup execute called.") rospy.sleep(1) self._as_move_group.set_aborted() return def blend_move_group_execute_callback(self, goal): rospy.loginfo("BlendMoveGroup execute called.") rospy.sleep(1) self._as_sequence_group.set_aborted() return
class ScanTableActionServer: """ Scan Table Mock Run this file to have mock to the action '/head_traj_controller/head_scan_snapshot_action '(Scan Table) """ def __init__(self): a_scan_table = {'name': '/head_traj_controller/head_scan_snapshot_action', 'ActionSpec': PointHeadAction, 'execute_cb': self.scan_table_cb, 'auto_start': False} self.s = SimpleActionServer(**a_scan_table) self.s.start() def scan_table_cb(self, req): rospy.loginfo('Scan Table \'/head_traj_controller/head_scan_snapshot_action was called.') self.s.set_succeeded() if bool(random.randint(0, 1)) else self.s.set_aborted()
class DataFusionServer(object): def __init__(self, name, topic, action_type, result_type): self.name = name self.result = result_type() self.topic = topic self.action_type = action_type self.timeout = 5 Subscriber('mock/' + name, String, self.receive_commands) self.server = ActionServer(self.topic, self.action_type, self.success, False) self.server.start() loginfo('>>> Starting ' + self.name) def receive_commands(self, msg): callback, timeout = msg.data.split(':') self.timeout = float(timeout) self.server.execute_callback = getattr(self, callback) logwarn('>>> ' + self.name + ': Current callback -> ' + callback) sleep(1) def create_random_world_model(self): victims = [create_victim_info(i + 1) for i in range(2)] visited = [create_victim_info(i + 1) for i in range(4)] self.result.worldModel = create_world_model(victims, visited) def success(self, goal): self.create_random_world_model() logwarn('>>> ' + self.name + ': This goal will succeed.') sleep(self.timeout) self.server.set_succeeded(result=self.result) def abort(self, goal): self.create_random_world_model() logwarn('>>> ' + self.name + ': This goal will be aborted.') sleep(self.timeout) self.server.set_aborted(result=self.result)
class OSMTopologicalPlannerNode(object): def __init__(self): server_ip = rospy.get_param('~overpass_server_ip') server_port = rospy.get_param('~overpass_server_port') ref_lat = rospy.get_param('~ref_latitude') ref_lon = rospy.get_param('~ref_longitude') building = rospy.get_param('~building') global_origin = [ref_lat, ref_lon] rospy.loginfo("Server " + server_ip + ":" + str(server_port)) rospy.loginfo("Global origin: " + str(global_origin)) rospy.loginfo("Starting servers...") self.osm_topological_planner_server = SimpleActionServer( '/osm_topological_planner', OSMTopologicalPlannerAction, self._osm_topological_planner, False) self.osm_topological_planner_server.start() osm_bridge = OSMBridge( server_ip=server_ip, server_port=server_port, global_origin=global_origin, coordinate_system="cartesian", debug=False) path_planner = PathPlanner(osm_bridge) path_planner.set_building(building) self.osm_topological_planner_callback = OSMTopologicalPlannerCallback( osm_bridge, path_planner) rospy.loginfo( "OSM topological planner server started. Listening for requests...") def _osm_topological_planner(self, req): res = self.osm_topological_planner_callback.get_safe_response(req) if res is not None: self.osm_topological_planner_server.set_succeeded(res) else: self.osm_topological_planner_server.set_aborted(res)
class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while (not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
class ReemTabletopManipulationMock(): def __init__(self): a_grasp_target_action = {'name': '/tabletop_grasping_node', 'ActionSpec': GraspTargetAction, 'execute_cb': self.grasp_target_action_cb, 'auto_start': False} self.s = SimpleActionServer(**a_grasp_target_action) self.s.start() def grasp_target_action_cb(self, req): rospy.loginfo('Grasp Target Action \'%s\' /tabletop_grasping_node was called.' % req.appearanceID) res = GraspTargetResult() res.detectionResult = TabletopDetectionResult() res.detectionResult.models = [DatabaseModelPoseList()] res.detectionResult.models[0].model_list = [DatabaseModelPose()]# = [0].model_id = req.databaseID res.detectionResult.models[0].model_list[0].model_id = req.databaseID self.s.set_succeeded(res) if bool(random.randint(0, 1)) else self.s.set_aborted(res)
class HokuyoLaserActionServer(): def __init__(self): # Initialize constants self.error_threshold = 0.0175 # Report success if error reaches below threshold self.signal = 1 # Initialize new node rospy.init_node(NAME, anonymous=True) controller_name = rospy.get_param('~controller') # Initialize publisher & subscriber for tilt self.laser_tilt = JointControllerState() self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64) self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal) self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True) rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state) rospy.wait_for_message(controller_name + '/state', JointControllerState) # Initialize tilt action server self.result = HokuyoLaserTiltResult() self.feedback = HokuyoLaserTiltFeedback() self.feedback.tilt_position = self.laser_tilt.process_value self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback) rospy.loginfo("%s: Ready to accept goals", NAME) def process_tilt_state(self, tilt_data): self.laser_tilt = tilt_data def reset_tilt_position(self, offset=0.0): self.laser_tilt_pub.publish(offset) rospy.sleep(0.5) def execute_callback(self, goal): r = rospy.Rate(100) self.joint_speed_srv(2.0) self.reset_tilt_position(goal.offset) delta = goal.amplitude - goal.offset target_speed = delta / goal.duration timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration) self.joint_speed_srv(target_speed) print "delta = %f, target_speed = %f" % (delta, target_speed) self.result.success = True self.result.tilt_position = self.laser_tilt.process_value rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles) # Tilt laser goal.tilt_cycles amount of times. for i in range(1, goal.tilt_cycles + 1): self.feedback.progress = i # Issue 2 commands for each cycle for j in range(2): if j % 2 == 0: target_tilt = goal.offset + goal.amplitude # Upper tilt limit self.signal = 0 else: target_tilt = goal.offset # Lower tilt limit self.signal = 1 # Publish target command to controller self.laser_tilt_pub.publish(target_tilt) start_time = rospy.Time.now() current_time = start_time while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold: #delta = abs(target_tilt - self.laser_tilt.process_value) #time_left = goal.duration - (rospy.Time.now() - start_time).to_sec() #target_speed = delta / time_left #self.joint_speed_srv(target_speed) # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() return # Publish current head position as feedback self.feedback.tilt_position = self.laser_tilt.process_value self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > timeout_threshold): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() return r.sleep() signal = LaserScannerSignal() signal.header.stamp = current_time signal.signal = self.signal self.laser_signal_pub.publish(signal) #rospy.sleep(0.5) if self.result.success: rospy.loginfo("%s: Goal Completed", NAME) self.result.tilt_position = self.laser_tilt.process_value self.server.set_succeeded(self.result)
class SocialGaze: '''Handles where to point robot's head''' def __init__(self): self._default_focus_point = Point(1, 0, 1.05) self._down_focus_point = Point(0.5, 0, 0.5) self._target_focus_point = Point(1, 0, 1.05) self._current_focus_point = Point(1, 0, 1.05) self._current_gaze_action = GazeGoal.LOOK_FORWARD self._prev_gaze_action = self._current_gaze_action self._prev_target_focus_point = array(self._default_focus_point) # Some constants self._no_interrupt = [GazeGoal.NOD, GazeGoal.SHAKE, GazeGoal.LOOK_DOWN] self._nod_positions = [Point(1, 0, 0.70), Point(1, 0, 1.20)] self._shake_positions = [Point(1, 0.2, 1.05), Point(1, -0.2, 1.05)] self._n_nods = 5 self._n_shakes = 5 self._nod_counter = 5 self._shake_counter = 5 self._face_pos = None self._glance_counter = 0 self.gaze_goal_strs = { GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD', GazeGoal.FOLLOW_EE: 'FOLLOW_EE', GazeGoal.GLANCE_EE: 'GLANCE_EE', GazeGoal.NOD: 'NOD', GazeGoal.SHAKE: 'SHAKE', GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE', GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT', GazeGoal.LOOK_DOWN: 'LOOK_DOWN', GazeGoal.NONE: 'NONE', } ## Action client for sending commands to the head. self._head_action_client = SimpleActionClient( '/head_controller/point_head', PointHeadAction) self._head_action_client.wait_for_server() rospy.loginfo('Head action client has responded.') self._head_goal = PointHeadGoal() self._head_goal.target.header.frame_id = 'base_link' self._head_goal.min_duration = rospy.Duration(1.0) self._head_goal.target.point = Point(1, 0, 1) ## Service client for arm states self._tf_listener = TransformListener() ## Server for gaze requested gaze actions self._gaze_action_server = SimpleActionServer( 'gaze_action', GazeAction, self._execute_gaze_action, False) self._gaze_action_server.start() self._is_action_complete = True rospy.Service('get_current_gaze_goal', GetGazeGoal, self._get_gaze_goal) # ################################################################## # Instance methods: Public (API) # ################################################################## def update(self): '''Update goal for head movement''' is_action_possibly_complete = True if self._current_gaze_action == GazeGoal.FOLLOW_EE: self._target_focus_point = self._get_ee_pos() elif self._current_gaze_action == GazeGoal.NOD: self._target_focus_point = self._get_next_nod_point( self._current_focus_point, self._target_focus_point) self._head_goal.min_duration = rospy.Duration(0.5) is_action_possibly_complete = False elif self._current_gaze_action == GazeGoal.SHAKE: self._target_focus_point = self._get_next_shake_point( self._current_focus_point, self._target_focus_point) self._head_goal.min_duration = rospy.Duration(0.5) is_action_possibly_complete = False elif self._current_gaze_action == GazeGoal.GLANCE_EE: self._target_focus_point = self._get_next_glance_point( self._current_focus_point, self._target_focus_point) is_action_possibly_complete = False self._current_focus_point = SocialGaze._filter_look_at_position( self._current_focus_point, self._target_focus_point) if self._current_gaze_action is GazeGoal.NONE: pass elif (SocialGaze._is_the_same( SocialGaze._point2array(self._head_goal.target.point), SocialGaze._point2array(self._target_focus_point))): if is_action_possibly_complete: head_state = self._head_action_client.get_state() if head_state == GoalStatus.SUCCEEDED: self._is_action_complete = True else: self._head_goal.target.point.x = self._current_focus_point.x self._head_goal.target.point.y = self._current_focus_point.y self._head_goal.target.point.z = self._current_focus_point.z self._head_action_client.send_goal(self._head_goal) time.sleep(0.02) # ################################################################## # Static methods: Internal ("private") # ################################################################## @staticmethod def _is_the_same(current, target): '''Get 3DOF pose of end effector Args: current (array): (x, y, z) of point target (array): (x, y, z) of point Returns: bool ''' diff = target - current dist = linalg.norm(diff) return dist < 0.003 @staticmethod def _point2array(p): '''Make Point msg into array Args: p (Point) Returns: array ''' return array((p.x, p.y, p.z)) @staticmethod def _array2point(a): '''Make array into Point msg Args: a (array) Returns: Point ''' return Point(a[0], a[1], a[2]) @staticmethod def _filter_look_at_position(current, target): '''If head goal is too far away, returns an intermediate position to limit speed Args: current (Point): current head goal target (Point): new head goal Returns: Point ''' speed = 0.02 diff = (SocialGaze._point2array(target) - SocialGaze._point2array(current)) dist = linalg.norm(diff) if dist > speed: step = dist / speed return SocialGaze._array2point(SocialGaze._point2array(current) + diff / step) else: return target # ################################################################## # Instance methods: Internal ("private") # ################################################################## def _get_gaze_goal(self, req): '''Return current gaze goal Args: req (GetGazeGoalRequest) : Unused Returns: GetGazeGoalResponse ''' goal = self._current_gaze_action # rospy.loginfo("Gaze Goal: {}".format(goal)) return GetGazeGoalResponse(int(goal)) def _get_ee_pos(self): '''Get 3DOF pose of end effector Returns: Point: location of wrist of end effector (could change to be finger tips) ''' from_frame = '/base_link' to_frame = '/wrist_roll_link' try: t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) (position, rotation) = self._tf_listener.lookupTransform( from_frame, to_frame, t) except: rospy.logerr('Could not get the end-effector pose.') return Point(position[0], position[1], position[2]) ## Callback function for receiving gaze commands def _execute_gaze_action(self, goal): '''Get 3DOF pose of end effector Args: goal (GazeGoal): what type of action to perform next ''' # rospy.loginfo("Got a head goal: {}".format(goal.action)) command = goal.action if self._no_interrupt.count(self._current_gaze_action) == 0: if (self._current_gaze_action != command or command == GazeGoal.LOOK_AT_POINT): self._is_action_complete = False if command == GazeGoal.LOOK_FORWARD: self._target_focus_point = self._default_focus_point elif command == GazeGoal.LOOK_DOWN: self._target_focus_point = self._down_focus_point elif command == GazeGoal.NOD: self._n_nods = goal.repeat self._start_nod() elif command == GazeGoal.SHAKE: self._n_shakes = goal.repeat self._start_shake() elif command == GazeGoal.GLANCE_EE: self._start_glance() elif command == GazeGoal.LOOK_AT_POINT: self._target_focus_point = goal.point # rospy.loginfo('\tSetting gaze action to: ' + # self.gaze_goal_strs[command]) self._current_gaze_action = command while not self._is_action_complete: time.sleep(0.1) self._current_gaze_action = GazeGoal.NONE # Perturb the head goal so it gets updated in the update loop. self._head_goal.target.point.x += 1 self._gaze_action_server.set_succeeded() else: self._gaze_action_server.set_aborted() else: self._gaze_action_server.set_aborted() def _start_nod(self): '''Start nod action''' self._prev_target_focus_point = self._target_focus_point self._prev_gaze_action = str(self._current_gaze_action) self._nod_counter = 0 self._target_focus_point = self._nod_positions[0] def _start_glance(self): '''Start glance action''' self._prev_target_focus_point = self._target_focus_point self._prev_gaze_action = str(self._current_gaze_action) self._glance_counter = 0 self._target_focus_point = self._get_ee_pos() def _start_shake(self): '''Start shake action''' self._prev_target_focus_point = self._target_focus_point self._prev_gaze_action = str(self._current_gaze_action) self._shake_counter = 0 self._target_focus_point = self._shake_positions[0] def _get_next_nod_point(self, current, target): '''Get next point to look at while nodding Args: current (Point): current head goal target (Point): new head goal Returns: Point ''' if (SocialGaze._is_the_same(SocialGaze._point2array(current), SocialGaze._point2array(target))): self._nod_counter += 1 if self._nod_counter == self._n_nods: self._current_gaze_action = self._prev_gaze_action return self._prev_target_focus_point else: return self._nod_positions[self._nod_counter % 2] else: return target def _get_next_glance_point(self, current, target): '''Get next point to look at while glancing Args: current (Point): current head goal target (Point): new head goal Returns: Point ''' if (SocialGaze._is_the_same(SocialGaze._point2array(current), SocialGaze._point2array(target))): self._glance_counter = 1 self._current_gaze_action = self._prev_gaze_action return self._prev_target_focus_point else: return target def _get_next_shake_point(self, current, target): '''Get next point to look at while shaking Args: current (Point): current head goal target (Point): new head goal Returns: Point ''' if (SocialGaze._is_the_same(SocialGaze._point2array(current), SocialGaze._point2array(target))): self._shake_counter += 1 if self._shake_counter == self._n_shakes: self._current_gaze_action = self._prev_gaze_action return self._prev_target_focus_point else: return self._shake_positions[self._shake_counter % 2] else: return target
class NavToNode(object): def __init__(self): # action client self._ac_gp = SimpleActionClient("global_planner_node_action", GlobalPlannerAction) self._ac_gp.wait_for_server() self._ac_lp = SimpleActionClient("local_planner_node_action", LocalPlannerAction) self._ac_lp.wait_for_server() # action servergoal self._as = SimpleActionServer("nav_to_node_action", NavToAction, self.ExecuteCB, auto_start=False) # define goals to send self.global_planner_goal_ = GlobalPlannerGoal() self.local_planner_goal_ = LocalPlannerGoal() self.new_goal_ = False self.new_path_ = False self.global_planner_error_code = -1 self.local_planner_error_code = -1 # new thread running the execution loop self.thread_ = Thread(target=self.Loop) self.thread_.start() self._as.start() def ExecuteCB(self, goal): rospy.loginfo('NAVTO: Goal received') self.global_planner_goal_.goal = goal.navgoal self.global_planner_error_code = -1 self.local_planner_error_code = -1 self.new_goal_ = True while not rospy.is_shutdown(): if self._as.is_preempt_requested(): rospy.loginfo('NAVTO: preempt requested') self._as.set_preempted() self.new_goal_ = False self.new_path_ = False self._ac_gp.cancel_all_goals() self._ac_lp.cancel_all_goals() return if self.global_planner_error_code == 0: self.new_goal_ = False self.new_path_ = False self._ac_lp.wait_for_result() self._as.set_succeeded() # rospy.loginfo('NAVTO: succeeded') break if self.global_planner_error_code >= 1: self.new_goal_ = False self.new_path_ = False self._ac_lp.wait_for_result() self._as.set_aborted() # rospy.loginfo('NAVTO: failed') break def Loop(self): rate = rospy.Rate(20) while not rospy.is_shutdown(): if self._as.is_preempt_requested(): self._ac_gp.cancel_all_goals() self._ac_lp.cancel_all_goals() if self.new_goal_: self._ac_gp.send_goal(self.global_planner_goal_, self.GlobalPlannerDoneCB, None, self.GlobalPlannerFeedbackCallback) self.new_goal_ = False if self.new_path_: self._ac_lp.send_goal(self.local_planner_goal_, self.LocalPlannerDoneCallback, None, self.LocalPlanerFeedbackCallback) self.new_path_ = False try: rate.sleep() except: rospy.loginfo('NAVTO: exiting') def GlobalPlannerDoneCB(self, state, result): rospy.loginfo("NAVTO: GP, %d, %d" % (state, result.error_code)) self.global_planner_error_code = result.error_code def GlobalPlannerFeedbackCallback(self, data): self.local_planner_goal_.route = data.path self.new_path_ = True def LocalPlannerDoneCallback(self, state, result): rospy.loginfo("NAVTO: LP, %d, %d" % (state, result.error_code)) self.local_planner_error_code = result.error_code def LocalPlanerFeedbackCallback(self, data): pass
class LiftObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording) self.posture_controller = SimpleActionClient("/wubble_gripper_grasp_action", GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient("/move_left_arm", MoveArmAction) self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject) self.action_server = SimpleActionServer( ACTION_NAME, LiftObjectAction, execute_cb=self.lift_object, auto_start=False ) def initialize(self): rospy.loginfo("%s: waiting for wubble_grasp_status service" % ACTION_NAME) rospy.wait_for_service("/wubble_grasp_status") rospy.loginfo("%s: connected to wubble_grasp_status service" % ACTION_NAME) rospy.loginfo("%s: waiting for wubble_gripper_grasp_action" % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo("%s: connected to wubble_gripper_grasp_action" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/start_audio_recording") rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/stop_audio_recording") rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for move_left_arm action server" % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo("%s: connected to move_left_arm action server" % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def lift_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_support_surface_name = goal.collision_support_surface_name # disable collisions between grasped object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = GRIPPER_GROUP_NAME collision_operation2.object2 = collision_support_surface_name collision_operation2.operation = CollisionOperation.DISABLE ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # this is a hack to make arm lift an object faster obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.REMOVE obj.object.id = collision_support_surface_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) joint_names = current_state.joint_names joint_positions = current_state.actual.positions start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS] start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit if not move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, start_angles, link_padding=gripper_paddings, collision_operations=ordered_collision_operations, ): self.action_server.set_aborted() return self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, LIFT_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations, ): # check if are still holding an object after lift is done grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr("%s: attempted lift failed" % ACTION_NAME) self.action_server.set_aborted() return
class MotionService(object): def __init__(self): rospy.init_node('motion_service') # Load config config_loader = RobotConfigLoader() try: robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config') except KeyError: rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for ' 'Thor Mang.') robot_config_name = 'thor' config_loader.load_xml_by_name(robot_config_name + '_config.xml') # Create publisher for first target if len(config_loader.targets) > 0: self._motion_publisher = MotionPublisher( config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix) else: rospy.logerr('Robot config needs to contain at least one valid target.') self._motion_data = MotionData(config_loader.robot_config) # Subscriber to start motions via topic self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb) # Create action server self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False) self._action_server.register_goal_callback(self._execute_motion_goal) self._action_server.register_preempt_callback(self._preempt_motion_goal) self._preempted = False def _execute_motion_goal(self): goal = self._action_server.accept_new_goal() # Check if motion exists if goal.motion_name not in self._motion_data: result = ExecuteMotionResult() result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN] result.error_string = "The requested motion is unknown." self._action_server.set_aborted(result) return # check if a valid time_factor is set, otherwise default to 1.0 if goal.time_factor <= 0.0: goal.time_factor = 1.0 self._preempted = False self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished) print '[MotionService] New action goal received.' def _preempt_motion_goal(self): self._motion_publisher.stop_motion() print '[MotionService] Preempting goal.' self._preempted = True def _trajectory_finished(self, error_codes, error_groups): result = ExecuteMotionResult() result.error_code = error_codes error_string = "" for error_code, error_group in zip(error_codes, error_groups): error_string += error_group + ': ' + str(error_code) + '\n' result.error_string = error_string if self._preempted: self._action_server.set_preempted(result) else: self._action_server.set_succeeded(result) print '[MotionService] Trajectory finished with error code: \n', error_string def _execute_motion(self, request): response = ExecuteMotionResponse() # check if a motion with this name exists if request.motion_name not in self._motion_data: print '[MotionService] Unknown motion name: "%s"' % request.motion_name response.ok = False response.finish_time.data = rospy.Time.now() return response # check if a valid time_factor is set, otherwise default to 1.0 if request.time_factor <= 0.0: request.time_factor = 1.0 # find the duration of the requested motion motion_duration = 0.0 for poses in self._motion_data[request.motion_name].values(): if len(poses) > 0: endtime = poses[-1]['starttime'] + poses[-1]['duration'] motion_duration = max(motion_duration, endtime) motion_duration *= request.time_factor # execute motion print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration) self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor) # reply with ok and the finish_time of this motion response.ok = True response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration) return response def start_server(self): rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion) self._action_server.start() print '[MotionService] Waiting for calls...' rospy.spin() def motion_command_request_cb(self, command): print "[MotionService] Initiate motion command via topic ..." request = ExecuteMotion() request.motion_name = command.motion_name request.time_factor = command.time_factor self._execute_motion(request) print "[MotionService] Motion command complete"
class SmartArmGripperActionServer(): def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for left finger self.left_finger_frame = 'arm_left_finger_link' self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64) rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger) rospy.wait_for_message('finger_left_controller/state', JointControllerState) # Initialize publisher & subscriber for right finger self.right_finger_frame = 'arm_right_finger_link' self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64) rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger) rospy.wait_for_message('finger_right_controller/state', JointControllerState) # Initialize action server self.result = SmartArmGripperResult() self.feedback = SmartArmGripperFeedback() self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback) # Reset gripper position rospy.sleep(1) self.reset_gripper_position() rospy.loginfo("%s: Ready to accept goals", NAME) def reset_gripper_position(self): self.left_finger_pub.publish(0.0) self.right_finger_pub.publish(0.0) rospy.sleep(5) def read_left_finger(self, data): self.left_finger = data self.has_latest_left_finger = True def read_right_finger(self, data): self.right_finger = data self.has_latest_right_finger = True def wait_for_latest_controller_states(self, timeout): self.has_latest_left_finger = False self.has_latest_right_finger = False r = rospy.Rate(100) start = rospy.Time.now() while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \ (rospy.Time.now() - start < timeout): r.sleep() def execute_callback(self, goal): r = rospy.Rate(100) self.result.success = True self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] rospy.loginfo("%s: Executing move gripper", NAME) # Initialize target joints target_joints = list() for i in range(self.JOINTS_COUNT): target_joints.append(0.0) # Retrieve target joints from goal if (len(goal.target_joints) > 0): for i in range(min(len(goal.target_joints), len(target_joints))): target_joints[i] = goal.target_joints[i] else: rospy.loginfo("%s: Aborted: Invalid Goal", NAME) self.result.success = False self.server.set_aborted() return # Publish goal to controllers self.left_finger_pub.publish(target_joints[0]) self.right_finger_pub.publish(target_joints[1]) # Initialize loop variables start_time = rospy.Time.now() while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD): # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() break # Publish current gripper position as feedback self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > self.TIMEOUT_THRESHOLD): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() break r.sleep() if (self.result.success): rospy.loginfo("%s: Goal Completed", NAME) self.wait_for_latest_controller_states(rospy.Duration(2.0)) self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value] self.server.set_succeeded(self.result)
class AbstractHMIServer(object): """ Abstract base class for a hmi client >>> class HMIServer(AbstractHMIServer): ... def __init__(self): ... pass ... def _determine_answer(self, description, spec, choices): ... return QueryResult() ... def _set_succeeded(self, result): ... print result >>> server = HMIServer() >>> from hmi_msgs.msg import QueryGoal >>> goal = QueryGoal(description='q', spec='spec', choices=[]) >>> server._execute_cb(goal) raw_result: '' results: [] >>> class HMIServer(AbstractHMIServer): ... def __init__(self): ... pass >>> server = HMIServer() Traceback (most recent call last): ... TypeError: Can't instantiate abstract class HMIServer with abstract methods _determine_answer """ __metaclass__ = ABCMeta def __init__(self, name): self._action_name = name self._server = SimpleActionServer(name, QueryAction, execute_cb=self._execute_cb, auto_start=False) self._server.start() rospy.loginfo('HMI server started on "%s"', name) def _execute_cb(self, goal): # TODO: refactor this somewhere choices = {} for choice in goal.choices: if choice.id in choices: rospy.logwarn('duplicate key "%s" in answer', choice.id) else: choices[choice.id] = choice.values rospy.loginfo('I got a question: %s', goal.description) rospy.loginfo('This is the spec: %s, %s', trim_string(goal.spec), repr(choices)) try: result = self._determine_answer(description=goal.description, spec=goal.spec, choices=choices, is_preempt_requested=self._server.is_preempt_requested) except Exception as e: # rospy.logwarn('_determine_answer raised an exception: %s', e) # import pdb; pdb.set_trace() tb = traceback.format_exc() rospy.logerr('_determine_answer raised an exception: %s' % tb) self._server.set_aborted() else: # we've got a result or a cancel if result: self._set_succeeded(result=result.to_ros(self._action_name)) rospy.loginfo('result: %s', result) else: rospy.loginfo('cancelled') self._server.set_aborted(text="Cancelled by user") def _set_succeeded(self, result): self._server.set_succeeded(result) def _publish_feedback(self): self._server.publish_feedback(QueryActionFeedback()) @abstractmethod def _determine_answer(self, description, spec, choices, is_preempt_requested): ''' Overwrite this method to provide custom implementations Return the answer Return None if nothing is heared Raise an Exception if an error occured ''' pass
class PushObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/stop_audio_recording', StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.interpolated_ik_params_srv = rospy.ServiceProxy( '/l_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams) self.interpolated_ik_srv = rospy.ServiceProxy( '/l_interpolated_ik_motion_plan', GetMotionPlan) self.set_planning_scene_diff_client = rospy.ServiceProxy( '/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_fk_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_fk', GetPositionFK) self.trajectory_filter_srv = rospy.ServiceProxy( '/trajectory_filter_unnormalizer/filter_trajectory', FilterJointTrajectory) self.trajectory_controller = SimpleActionClient( '/l_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PushObjectAction, execute_cb=self.push_object, auto_start=False) def initialize(self): rospy.loginfo( '%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo( '%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo( '%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for l_interpolated_ik_motion_plan_set_params service' % ACTION_NAME) rospy.wait_for_service('/l_interpolated_ik_motion_plan_set_params') rospy.loginfo( '%s: connected to l_interpolated_ik_motion_plan_set_params service' % ACTION_NAME) rospy.loginfo('%s: waiting for l_interpolated_ik_motion_plan service' % ACTION_NAME) rospy.wait_for_service('/l_interpolated_ik_motion_plan') rospy.loginfo( '%s: connected to l_interpolated_ik_motion_plan service' % ACTION_NAME) rospy.loginfo( '%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME) rospy.wait_for_service('/environment_server/set_planning_scene_diff') rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME) rospy.loginfo( '%s: waiting for wubble2_left_arm_kinematics/get_fk service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_fk') rospy.loginfo( '%s: connected to wubble2_left_arm_kinematics/get_fk service' % ACTION_NAME) rospy.loginfo( '%s: waiting for trajectory_filter_unnormalizer/filter_trajectory service' % ACTION_NAME) rospy.wait_for_service( '/trajectory_filter_unnormalizer/filter_trajectory') rospy.loginfo( '%s: connected to trajectory_filter_unnormalizer/filter_trajectory service' % ACTION_NAME) rospy.loginfo( '%s: waiting for l_arm_controller/follow_joint_trajectory' % ACTION_NAME) self.trajectory_controller.wait_for_server() rospy.loginfo( '%s: connected to l_arm_controller/follow_joint_trajectory' % ACTION_NAME) self.action_server.start() def create_pose_stamped(self, pose, frame_id='base_link'): """ Creates a PoseStamped message from a list of 7 numbers (first three are position and next four are orientation: pose = [px,py,pz, ox,oy,oz,ow] """ m = PoseStamped() m.header.frame_id = frame_id m.header.stamp = rospy.Time() m.pose.position = Point(*pose[0:3]) m.pose.orientation = Quaternion(*pose[3:7]) return m #pretty-print list to string def pplist(self, list_to_print): return ' '.join(['%5.3f' % x for x in list_to_print]) def get_interpolated_ik_motion_plan(self, start_pose, goal_pose, start_angles, joint_names, pos_spacing=0.01, rot_spacing=0.1, consistent_angle=math.pi / 9, collision_aware=True, collision_check_resolution=1, steps_before_abort=-1, num_steps=0, ordered_collision_operations=None, frame='base_footprint', start_from_end=0, max_joint_vels=[1.5] * 7, max_joint_accs=[8.0] * 7): res = self.interpolated_ik_params_srv(num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs) req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses = [ start_pose.pose ] req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [ GRIPPER_LINK_FRAME ] req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [ start_pose.header.frame_id ] pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [ pos_constraint, ] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [ orient_constraint, ] #req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] #req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) #if ordered_collision_operations is not None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations res = self.interpolated_ik_srv(req) return res def check_cartesian_path_lists(self, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing=0.03, rot_spacing=0.1, consistent_angle=math.pi / 7.0, collision_aware=True, collision_check_resolution=1, steps_before_abort=1, num_steps=0, ordered_collision_operations=None, frame='base_link'): start_pose = self.create_pose_stamped(approachpos + approachquat, frame) goal_pose = self.create_pose_stamped(grasppos + graspquat, frame) return self.get_interpolated_ik_motion_plan( start_pose, goal_pose, start_angles, ARM_JOINTS, pos_spacing, rot_spacing, consistent_angle, collision_aware, collision_check_resolution, steps_before_abort, num_steps, ordered_collision_operations, frame) def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message('/joint_states', JointState) req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = 'base_link' approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] push_distance = 0.40 grasppos = [ pose.position.x, pose.position.y - push_distance, pose.position.z ] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4 } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [ res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len) ] vels = [ res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len) ] times = [ res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len) ] error_codes = [ error_code_dict[error_code.val] for error_code in res.trajectory_error_codes ] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[ 1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
class PushObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus) self.interpolated_ik_params_srv = rospy.ServiceProxy( "/l_interpolated_ik_motion_plan_set_params", SetInterpolatedIKMotionPlanParams ) self.interpolated_ik_srv = rospy.ServiceProxy("/l_interpolated_ik_motion_plan", GetMotionPlan) self.set_planning_scene_diff_client = rospy.ServiceProxy( "/environment_server/set_planning_scene_diff", SetPlanningSceneDiff ) self.get_fk_srv = rospy.ServiceProxy("/wubble2_left_arm_kinematics/get_fk", GetPositionFK) self.trajectory_filter_srv = rospy.ServiceProxy( "/trajectory_filter_unnormalizer/filter_trajectory", FilterJointTrajectory ) self.trajectory_controller = SimpleActionClient( "/l_arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction ) self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject) self.action_server = SimpleActionServer( ACTION_NAME, PushObjectAction, execute_cb=self.push_object, auto_start=False ) def initialize(self): rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/start_audio_recording") rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.wait_for_service("audio_dump/stop_audio_recording") rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME) rospy.loginfo("%s: waiting for l_interpolated_ik_motion_plan_set_params service" % ACTION_NAME) rospy.wait_for_service("/l_interpolated_ik_motion_plan_set_params") rospy.loginfo("%s: connected to l_interpolated_ik_motion_plan_set_params service" % ACTION_NAME) rospy.loginfo("%s: waiting for l_interpolated_ik_motion_plan service" % ACTION_NAME) rospy.wait_for_service("/l_interpolated_ik_motion_plan") rospy.loginfo("%s: connected to l_interpolated_ik_motion_plan service" % ACTION_NAME) rospy.loginfo("%s: waiting for environment_server/set_planning_scene_diff service" % ACTION_NAME) rospy.wait_for_service("/environment_server/set_planning_scene_diff") rospy.loginfo("%s: connected to set_planning_scene_diff service" % ACTION_NAME) rospy.loginfo("%s: waiting for wubble2_left_arm_kinematics/get_fk service" % ACTION_NAME) rospy.wait_for_service("/wubble2_left_arm_kinematics/get_fk") rospy.loginfo("%s: connected to wubble2_left_arm_kinematics/get_fk service" % ACTION_NAME) rospy.loginfo("%s: waiting for trajectory_filter_unnormalizer/filter_trajectory service" % ACTION_NAME) rospy.wait_for_service("/trajectory_filter_unnormalizer/filter_trajectory") rospy.loginfo("%s: connected to trajectory_filter_unnormalizer/filter_trajectory service" % ACTION_NAME) rospy.loginfo("%s: waiting for l_arm_controller/follow_joint_trajectory" % ACTION_NAME) self.trajectory_controller.wait_for_server() rospy.loginfo("%s: connected to l_arm_controller/follow_joint_trajectory" % ACTION_NAME) self.action_server.start() def create_pose_stamped(self, pose, frame_id="base_link"): """ Creates a PoseStamped message from a list of 7 numbers (first three are position and next four are orientation: pose = [px,py,pz, ox,oy,oz,ow] """ m = PoseStamped() m.header.frame_id = frame_id m.header.stamp = rospy.Time() m.pose.position = Point(*pose[0:3]) m.pose.orientation = Quaternion(*pose[3:7]) return m # pretty-print list to string def pplist(self, list_to_print): return " ".join(["%5.3f" % x for x in list_to_print]) def get_interpolated_ik_motion_plan( self, start_pose, goal_pose, start_angles, joint_names, pos_spacing=0.01, rot_spacing=0.1, consistent_angle=math.pi / 9, collision_aware=True, collision_check_resolution=1, steps_before_abort=-1, num_steps=0, ordered_collision_operations=None, frame="base_footprint", start_from_end=0, max_joint_vels=[1.5] * 7, max_joint_accs=[8.0] * 7, ): res = self.interpolated_ik_params_srv( num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs, ) req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose] req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [GRIPPER_LINK_FRAME] req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id] pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint] # req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] # req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) # if ordered_collision_operations is not None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations res = self.interpolated_ik_srv(req) return res def check_cartesian_path_lists( self, approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing=0.03, rot_spacing=0.1, consistent_angle=math.pi / 7.0, collision_aware=True, collision_check_resolution=1, steps_before_abort=1, num_steps=0, ordered_collision_operations=None, frame="base_link", ): start_pose = self.create_pose_stamped(approachpos + approachquat, frame) goal_pose = self.create_pose_stamped(grasppos + graspquat, frame) return self.get_interpolated_ik_motion_plan( start_pose, goal_pose, start_angles, ARM_JOINTS, pos_spacing, rot_spacing, consistent_angle, collision_aware, collision_check_resolution, steps_before_abort, num_steps, ordered_collision_operations, frame, ) def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo("%s: preempted" % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message("/joint_states", JointState) req = GetPositionFKRequest() req.header.frame_id = "base_link" req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = "base_link" approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] push_distance = 0.40 grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations, ) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4, } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)] vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)] times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)] error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr("%s: attempted push failed" % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
class PickNPlaceServer(object): def __init__(self): rospy.loginfo('initializing PickNPlaceServer') # loading config variables self.dynamic_reconfigure_srv = Server(GraspConfig, self.dynamic_reconfigure_cb) # grasp library rospy.loginfo('connecting to pickup action server') self.pickup_ac = SimpleActionClient('/pickup', PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo('successfully connected to pickup action server!') rospy.loginfo('connecting to place action server') self.place_ac = SimpleActionClient('/place', PlaceAction) self.place_ac.wait_for_server() rospy.loginfo('successfully connected to place action server!') self.scene = PlanningSceneInterface() rospy.loginfo('connecting to /get_planning_scene service') self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo('successfully connected to get_planning_scene service!') rospy.loginfo('connecting to clear octomap service') self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo('successfully connected to clear octomap service!') self.links_to_allow_contact = rospy.get_param( '/links_to_allow_contact') if not self.links_to_allow_contact: rospy.logwarn( "Didn't find any links to allow contacts.. at param ~links_to_allow_contact" ) else: rospy.loginfo('Found links to allow contacts : ' + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer('/pickup_pose', PickNPlacePoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer('/place_pose', PickNPlacePoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start() self.grasp_marker_pub = rospy.Publisher('/grasp_marker', Marker, queue_size=10) self.move_group = MoveGroupCommander('arm_torso') self.rate = rospy.Rate(10) self.velocity = Twist() self.velocity_publisher = rospy.Publisher( '/mobile_base_controller/cmd_vel', Twist, queue_size=10) def dynamic_reconfigure_cb(self, config, level): # load config values for grasping self._time_pre_grasp_posture = config["time_pre_grasp_posture"] self._time_grasp_posture = config["time_grasp_posture"] self._time_grasp_posture_final = config["time_grasp_posture_final"] self._max_contact_force = config["max_contact_force"] print(config) return config def pick_cb(self, goal): # check result from grasp_object and set it error_code = self.grasp_object(goal) pick_result = PickNPlacePoseResult() pick_result.error_code = error_code if error_code != 1: self.pick_as.set_aborted(pick_result) else: self.pick_as.set_succeeded(pick_result) def place_cb(self, goal): # check result from place_object and set it error_code = self.place_object(goal.object_pose) place_result = PickNPlacePoseResult() place_result.error_code = error_code if error_code != 1: self.place_as.set_aborted(place_result) else: self.place_as.set_succeeded(place_result) def wait_for_planning_scene_object(self, object_name='part'): rospy.loginfo('waiting for object ' + object_name + ' to appear in planning scene...') get_planning_scene_request = GetPlanningSceneRequest() get_planning_scene_request.components.components = get_planning_scene_request.components.WORLD_OBJECT_NAMES # loop until our object appears on scene part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: get_planning_scene_response = self.scene_srv.call( get_planning_scene_request) for collision_object in get_planning_scene_response.scene.world.collision_objects: if collision_object.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo(object_name + ' is in the scene!') def open_and_close_gripper(self, grasp_width): # open or close grippers pre_grasp_posture = JointTrajectory() pre_grasp_posture.header.frame_id = 'arm_tool_link' pre_grasp_posture.joint_names = [ 'gripper_left_finger_joint', 'gripper_right_finger_joint' ] jt_point1 = JointTrajectoryPoint() jt_point1.positions = [0.05, 0.05] jt_point1.time_from_start = rospy.Duration( self._time_pre_grasp_posture) pre_grasp_posture.points.append(jt_point1) grasp_posture = deepcopy(pre_grasp_posture) grasp_posture.points[0].time_from_start = rospy.Duration( self._time_pre_grasp_posture + self._time_grasp_posture) jt_point2 = JointTrajectoryPoint() jt_point2.positions = [grasp_width, grasp_width] jt_point2.time_from_start = rospy.Duration( self._time_pre_grasp_posture + self._time_grasp_posture + self._time_grasp_posture_final) grasp_posture.points.append(jt_point2) return { 'pre_grasp_posture': pre_grasp_posture, 'grasp_posture': grasp_posture } def create_gripper_translation(self, direction_vector, min_distance, desired_distance, frame_id='base_footprint'): # used to generate pre-grasp approach and post-grasp retreat for grasps g_trans = GripperTranslation() g_trans.direction.header.frame_id = frame_id g_trans.direction.vector = direction_vector g_trans.min_distance = min_distance g_trans.desired_distance = desired_distance return g_trans def create_grasp(self, pose, approach, retreat, grasp_width, grasp_id=''): g = Grasp() g.id = grasp_id g.grasp_pose.header.frame_id = 'base_footprint' g.grasp_pose.pose = pose # set direction of approach and retreat pre and post grasping, respectively g.pre_grasp_approach = self.create_gripper_translation( approach, 0.095, 0.9) g.post_grasp_retreat = self.create_gripper_translation( retreat, 0.1, 0.2) print('grasp width : ', grasp_width) gripper_postures = self.open_and_close_gripper(grasp_width) g.pre_grasp_posture = gripper_postures[ 'pre_grasp_posture'] # open gripper before grasp g.grasp_posture = gripper_postures[ 'grasp_posture'] # close gripper during grasp g.max_contact_force = self._max_contact_force # don't knock the object down whilst grasping return g def generate_grasps(self, pnp_goal): # decide between front or top grasp eef_gripper_dist = 0.11 gripper_height = 0.11 # objects must be equal or above this for front-grasp max_graspable = 0.10 # max obj dimension that can fit between the grippers grasp_depth = 0.06 # max depth between gripper tip to gripper palm grasps = [] if (pnp_goal.object_width < max_graspable and pnp_goal.object_height >= gripper_height): front_grasp_center_pose = Pose() front_grasp_center_pose.position = copy.deepcopy( pnp_goal.object_pose.pose.position) front_grasp_center_pose.orientation = Quaternion( *quaternion_from_euler(PI / 2, 0.0, 0.0)) # horizontal gripper front_grasp_center_pose.position.x = front_grasp_center_pose.position.x - ( eef_gripper_dist + (pnp_goal.object_depth / 2)) front_center_approach = Vector3( 1.0, 0.0, 0.0) # approach 'in' for front-grasp front_center_retreat = Vector3(0.0, 0.0, 1.0) # retreat 'upwards' post-grasp front_center_grasp_width = pnp_goal.object_width / 2.58 front_center_grasp_width = floor( front_center_grasp_width * 100) / 100 front_center_grasp = self.create_grasp( front_grasp_center_pose, front_center_approach, front_center_retreat, front_center_grasp_width, grasp_id='front_center_grasp') grasps.append(front_center_grasp) if (pnp_goal.object_width < max_graspable and pnp_goal.object_height > (gripper_height / 2.0)): front_grasp_top_pose = Pose() front_grasp_top_pose.position = copy.deepcopy( pnp_goal.object_pose.pose.position) front_grasp_top_pose.orientation = Quaternion( *quaternion_from_euler(PI / 2, 0.0, 0.0)) # horizontal gripper front_grasp_top_pose.position.x = front_grasp_top_pose.position.x - ( eef_gripper_dist + (pnp_goal.object_depth / 2)) front_grasp_top_pose.position.z = front_grasp_top_pose.position.z + ( pnp_goal.object_height / 2) - 0.05 front_top_approach = Vector3(1.0, 0.0, 0.0) # approach 'in' for front-grasp front_top_retreat = Vector3(0.0, 0.0, 1.0) # retreat 'upwards' post-grasp front_top_grasp_width = pnp_goal.object_width / 2.58 front_top_grasp_width = floor(front_top_grasp_width * 100) / 100 front_top_grasp = self.create_grasp(front_grasp_top_pose, front_top_approach, front_top_retreat, front_top_grasp_width, grasp_id='front_top_grasp') grasps.append(front_top_grasp) if (pnp_goal.object_width < max_graspable): top_grasp_width_pose = Pose() top_grasp_width_pose.position = copy.deepcopy( pnp_goal.object_pose.pose.position) top_grasp_width_pose.orientation = Quaternion( *quaternion_from_euler(PI / 2, PI / 2, 0.0)) top_grasp_width_pose.position.z = top_grasp_width_pose.position.z + ( grasp_depth + eef_gripper_dist + (pnp_goal.object_height / 2)) top_grasp_width_approach = Vector3( 0.0, 0.0, -1.0) # approach 'down' for top-grasp top_grasp_width_retreat = Vector3( 0.0, 0.0, 1.0) # retreat 'upwards' post-grasp top_grasp_width_width = pnp_goal.object_width / 2.58 top_grasp_width_width = floor(top_grasp_width_width * 100) / 100 top_grasp_width = self.create_grasp(top_grasp_width_pose, top_grasp_width_approach, top_grasp_width_retreat, top_grasp_width_width, grasp_id='top_grasp_width') grasps.append(top_grasp_width) if (pnp_goal.object_depth < max_graspable): top_grasp_depth_pose = Pose() top_grasp_depth_pose.position = copy.deepcopy( pnp_goal.object_pose.pose.position) top_grasp_depth_pose.orientation = Quaternion( *quaternion_from_euler(0.0, PI / 2, 0.0)) top_grasp_depth_pose.position.z = top_grasp_depth_pose.position.z + ( grasp_depth + eef_gripper_dist + (pnp_goal.object_height / 2)) top_grasp_depth_approach = Vector3( 0.0, 0.0, -1.0) # approach 'down' for top-grasp top_grasp_depth_retreat = Vector3( 0.0, 0.0, 1.0) # retreat 'upwards' post-grasp top_grasp_depth_width = pnp_goal.object_depth / 2.58 top_grasp_depth_width = floor(top_grasp_depth_width * 100) / 100 top_grasp_depth = self.create_grasp(top_grasp_depth_pose, top_grasp_depth_approach, top_grasp_depth_retreat, top_grasp_depth_width, grasp_id='top_grasp_depth') grasps.append(top_grasp_depth) return grasps def clear_world_objects(self): rospy.loginfo("removing any previous 'part' object") # self.scene.remove_attached_object('arm_tool_link') self.scene.remove_world_object('part') # def set_orientation_constraint(self, orientation, frame_id='base_footprint', link_name='arm_tool_link'): # gripper_constraint = Constraints() # gripper_constraint.name = 'gripper constraint' # # gripper_orientation_constraint = OrientationConstraint() # gripper_orientation_constraint.header.frame_id = frame_id # gripper_orientation_constraint.link_name = link_name # gripper_orientation_constraint.orientation = orientation # gripper_orientation_constraint.absolute_x_axis_tolerance = 0.01 # allow max rotation of 1 degree # gripper_orientation_constraint.absolute_y_axis_tolerance = 0.01 # gripper_orientation_constraint.absolute_z_axis_tolerance = 0.01 # gripper_orientation_constraint.weight = 1.0 # # gripper_constraint.orientation_constraints = [gripper_orientation_constraint] # # self.move_group.set_path_constraints(gripper_constraint) def execute_post_grasp_goal(self, grasp): # create arm goal with same 'arm_tool' orientation as the attempted grasp goal_pose = PoseStamped() goal_pose.header.frame_id = 'base_footprint' goal_pose.pose.position.x = 0.35 goal_pose.pose.position.y = -0.30 goal_pose.pose.position.z = 0.76 goal_pose.pose.orientation = grasp.grasp_pose.pose.orientation # set reference frame and send goal self.move_group.set_pose_reference_frame('base_footprint') self.move_group.set_pose_target(goal_pose) self.move_group.go() def move(self, linear=(0, 0, 0), angular=(0, 0, 0)): self.velocity.linear.x = linear[ 0] # Forward or Backward with in m/sec. self.velocity.linear.y = linear[1] self.velocity.linear.z = linear[2] self.velocity.angular.x = angular[0] self.velocity.angular.y = angular[1] self.velocity.angular.z = angular[ 2] # Anti-clockwise/clockwise in radians per sec self.velocity_publisher.publish(self.velocity) def grasp_object(self, pnp_goal): self.clear_world_objects() rospy.loginfo("adding new 'part' object") self.scene.add_box('part', pnp_goal.object_pose, (pnp_goal.object_depth, pnp_goal.object_width, pnp_goal.object_height)) # wait for objects part to appear self.wait_for_planning_scene_object('part') # compute grasp grasps = self.generate_grasps(pnp_goal) rospy.loginfo('computed grasps :: ') rospy.loginfo(grasps) error_code = -1 for grasp in grasps: # publish grasp marker print('publishing grasp marker! ' + grasp.id) if grasp.id.split('_')[0] != 'front': grasp_marker = Util.visualize_marker(grasp.grasp_pose.pose, scale=0.02, type='pose', color='blue') else: grasp_marker = Util.visualize_marker(grasp.grasp_pose.pose, scale=0.02, type='pose') self.grasp_marker_pub.publish(grasp_marker) goal = create_pickup_goal('arm_torso', 'part', pnp_goal.object_pose, [grasp], self.links_to_allow_contact) rospy.loginfo('sending pick goal') self.pickup_ac.send_goal(goal) rospy.loginfo('waiting for result') self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() error_code = result.error_code.val rospy.logdebug('using torso result: ' + str(result)) rospy.loginfo('pick result: ' + str(moveit_error_dict[error_code])) if error_code == 1: # turn around for about 180 degs to be clear of furniture for i in range(85): self.move((0, 0, 0), (0, 0, 0.4)) self.rate.sleep() # move arm close to body after grasp is successful self.execute_post_grasp_goal(grasp) break return error_code
class PutDownAtActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/stop_audio_recording', StopAudioRecording) self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning) self.get_solver_info_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo) self.get_ik_srv = rospy.ServiceProxy( '/wubble2_left_arm_kinematics/get_ik', GetPositionIK) self.set_planning_scene_diff_client = rospy.ServiceProxy( '/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.find_cluster_bbox = rospy.ServiceProxy( '/find_cluster_bounding_box', FindClusterBoundingBox) self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PutDownAtAction, execute_cb=self.put_down_at, auto_start=False) def initialize(self): rospy.loginfo( '%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo( '%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo( '%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME) rospy.wait_for_service('/GraspPlanning') rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME) rospy.loginfo( '%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.wait_for_service( '/wubble2_left_arm_kinematics/get_ik_solver_info') rospy.loginfo( '%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.loginfo( '%s: waiting for wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik') rospy.loginfo( '%s: connected to wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.loginfo( '%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME) rospy.wait_for_service('/environment_server/set_planning_scene_diff') rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) rospy.loginfo('%s: waiting for find_cluster_bounding_box service' % ACTION_NAME) rospy.wait_for_service('/find_cluster_bounding_box') rospy.loginfo('%s: connected to find_cluster_bounding_box service' % ACTION_NAME) self.action_server.start() def find_ik_for_grasping_pose(self, pose_stamped): solver_info = self.get_solver_info_srv() arm_joints = solver_info.kinematic_solver_info.joint_names req = GetPositionIKRequest() req.timeout = rospy.Duration(5.0) req.ik_request.ik_link_name = GRIPPER_LINK_FRAME req.ik_request.pose_stamped = pose_stamped try: current_state = rospy.wait_for_message('/joint_states', JointState, 2.0) req.ik_request.ik_seed_state.joint_state.name = arm_joints req.ik_request.ik_seed_state.joint_state.position = [ current_state.position[current_state.name.index(j)] for j in arm_joints ] if not self.set_planning_scene_diff_client(): rospy.logerr( '%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME) return None ik_result = self.get_ik_srv(req) if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS: return ik_result.solution else: rospy.logerr( '%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME) return None except: rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME) return None def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''): """ target = GraspableObject collision_object_name = name of target in collision map collision_support_surface_name = name of surface target is touching """ req = GraspPlanningRequest() req.arm_name = ARM_GROUP_NAME req.target = target req.collision_object_name = collision_object_name req.collision_support_surface_name = collision_support_surface_name rospy.loginfo( '%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name)) grasping_result = self.grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME) return None pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[ 0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME) return self.find_ik_for_grasping_pose(pose_stamped) def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() rospy.sleep(1) grasp_status = self.get_grasp_status_srv() return grasp_status.is_hand_occupied def find_cluster_bounding_box_center(self, cluster): fcbb = FindClusterBoundingBoxRequest() fcbb.cluster = cluster res = self.find_cluster_bbox(fcbb) if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR: rospy.logerr( 'Unable to find bounding box for requested point cluster') return None return res.pose.pose.position def put_down_at(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() bbox_center = self.find_cluster_bounding_box_center( goal.graspable_object.cluster) if not bbox_center: self.action_server.set_aborted() goal.target_location.z = bbox_center.z x_dist = goal.target_location.x - bbox_center.x y_dist = goal.target_location.y - bbox_center.y target = goal.graspable_object target.cluster.points = [ Point32(p.x + x_dist, p.y + y_dist, p.z) for p in target.cluster.points ] collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations.collision_operations = [ collision_operation, collision_operation2, collision_operation3 ] # set gripper padding to 0 gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS]) # move into pre-grasp pose if not move_arm_joint_goal( self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record put down sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded( PutDownAtResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted put down failed' % ACTION_NAME) self.action_server.set_aborted()
class FakeGrasping: ALWAYS = 0 NEVER = 1 RANDOM = 2 def __init__(self): self.server = SimpleActionServer('/robot_move', RobotMoveAction, execute_cb=self.robot_move_cb) self.objects = self.ALWAYS self.grasp = self.ALWAYS self.place = self.ALWAYS self.object_randomness = 0.8 # 80% of time object is known self.grasp_randomness = 0.4 self.place_randomness = 0.4 self.holding = None self.pick_length = 2 # how long (sec) takes to pick an object self.place_length = 2 # how long (sec) takes to place an object self.robot_state_pub = rospy.Publisher("/robot_status", RobotStatus, queue_size=1, latch=True) self.publish_robot_status(RobotStatus.STOPPED) random.seed() def publish_robot_status(self, status): self.robot_state_pub.publish( RobotStatus( status=status, actual_pose=PoseStamped(header=Header(frame_id="marker")))) def robot_move_cb(self, goal): self.publish_robot_status(RobotStatus.MOVING) self.robot_move(goal) self.publish_robot_status(RobotStatus.STOPPED) def robot_move(self, goal): result = RobotMoveResult() feedback = RobotMoveFeedback() if not (goal.move_type in (goal.HOME, goal.PROGRAMING, goal.PICK, goal.PLACE, goal.MOVE)): result.result = RobotMoveResult.BAD_REQUEST rospy.logerr("BAD_REQUEST, Unknown operation") self.server.set_aborted(result, "Unknown operation") return if self.objects == self.ALWAYS: pass elif self.objects == self.NEVER: result.result = RobotMoveResult.BAD_REQUEST rospy.logerr("BAD_REQUEST, Unknown object id") self.server.set_aborted(result, "Unknown object id") return elif self.objects == self.RANDOM: nmbr = random.random() if nmbr > self.object_randomness: result.result = RobotMoveResult.BAD_REQUEST rospy.logerr("BAD_REQUEST, Unknown object id") self.server.set_aborted(result, "Unknown object id") return grasped = False if goal.move_type == RobotMoveGoal.PICK: rospy.sleep(self.pick_length) if False and self.holding: result.result = RobotMoveResult.BUSY rospy.logerr("Failure, already holding object in arm") self.server.set_aborted(result, "Already holding object in arm") return if self.grasp == self.ALWAYS: grasped = True pass elif self.grasp == self.NEVER: result.result = RobotMoveResult.MOVE_FAILURE rospy.logerr("FAILURE, Pick Failed") self.server.set_aborted(result, "Pick Failed") return tries = 5 while tries > 0: feedback.state = feedback.PICKING tries -= 1 self.server.publish_feedback(feedback) if self.grasp == self.RANDOM: nmbr = random.random() if nmbr < self.grasp_randomness: grasped = True if grasped: break if self.server.is_preempt_requested(): self.server.set_preempted(result, "Pick canceled") rospy.logerr("Preempted") return if not grasped: result.result = RobotMoveResult.FAILURE self.server.set_aborted(result, "Pick failed") rospy.logerr("FAILURE, Pick Failed") return else: self.holding = True placed = False if goal.move_type == RobotMoveGoal.PLACE: rospy.sleep(self.place_length) if not self.holding: result.result = RobotMoveResult.MOVE_FAILURE rospy.logerr("Failure, robot not holding object") self.server.set_aborted(result, "Robot not holding object") return if self.place == self.ALWAYS: placed = True pass elif self.place == self.NEVER: result.result = RobotMoveResult.MOVE_FAILURE self.server.set_aborted(result, "Place Failed") rospy.logerr("FAILURE, Place Failed") return tries = 5 while tries > 0: feedback.state = feedback.PLACING tries -= 1 self.server.publish_feedback(feedback) if self.place == self.RANDOM: nmbr = random.random() if nmbr < self.place_randomness: placed = True if placed: break if not placed: result.result = RobotMoveResult.MOVE_FAILURE self.server.set_aborted(result, "Place failed") rospy.logerr("FAILURE, Place Failed") return else: self.holding = False result.result = RobotMoveResult.SUCCES self.server.set_succeeded(result) rospy.loginfo("SUCCESS") print("Finished")
class PlaceObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PlaceObjectAction, execute_cb=self.place_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal(self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded(PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
class WubbleGripperGraspController: def __init__(self): self.object_presence_pressure_threshold = rospy.get_param('object_presence_pressure_threshold', 200.0) self.object_presence_opening_threshold = rospy.get_param('object_presence_opening_threshold', 0.02) gripper_action_name = rospy.get_param('gripper_action_name', 'wubble_gripper_command_action') self.gripper_action_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction) # # while not rospy.is_shutdown(): # try: # self.gripper_action_client.wait_for_server(timeout=rospy.Duration(2.0)) # break # except ROSException as e: # rospy.loginfo('Waiting for %s action' % gripper_action_name) # except: # rospy.logerr('Unexpected error') # raise rospy.loginfo('Using gripper action client on topic %s' % gripper_action_name) query_service_name = rospy.get_param('grasp_query_name', 'wubble_grasp_status') self.query_srv = rospy.Service(query_service_name, GraspStatus, self.process_grasp_status) posture_action_name = rospy.get_param('posture_action_name', 'wubble_gripper_grasp_action') self.action_server = SimpleActionServer(posture_action_name, GraspHandPostureExecutionAction, self.process_grasp_action, False) self.action_server.start() rospy.loginfo('wubble_gripper grasp hand posture action server started on topic %s' % posture_action_name) def process_grasp_action(self, msg): gripper_command = WubbleGripperGoal() if msg.goal == GraspHandPostureExecutionGoal.GRASP: rospy.loginfo('Received GRASP request') # if not msg.goal.grasp.position: # msg = 'wubble gripper grasp execution: position vector empty in requested grasp' # rospy.logerr(msg) # self.action_server.set_aborted(text=msg) # return # gripper_command.command = WubbleGripperGoal.CLOSE_GRIPPER gripper_command.torque_limit = 0.4 gripper_command.dynamic_torque_control = True gripper_command.pressure_upper = 1900.0 gripper_command.pressure_lower = 1800.0 elif msg.goal == GraspHandPostureExecutionGoal.PRE_GRASP: rospy.loginfo('Received PRE_GRASP request') gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER gripper_command.torque_limit = 0.6 elif msg.goal == GraspHandPostureExecutionGoal.RELEASE: rospy.loginfo('Received RELEASE request') gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER gripper_command.torque_limit = 0.6 else: msg = 'wubble gripper grasp execution: unknown goal code (%d)' % msg.goal rospy.logerr(msg) self.action_server.set_aborted(text=msg) self.gripper_action_client.send_goal(gripper_command) self.gripper_action_client.wait_for_result() self.action_server.set_succeeded() def process_grasp_status(self, msg): result = GraspStatus() pressure_msg = rospy.wait_for_message('/total_pressure', Float64) opening_msg = rospy.wait_for_message('/gripper_opening', Float64) left_pos = rospy.wait_for_message('/left_finger_controller/state', JointState).position right_pos = rospy.wait_for_message('/right_finger_controller/state', JointState).position if pressure_msg.data <= self.object_presence_pressure_threshold: rospy.loginfo('Gripper grasp query false: gripper total pressure is below threshold (%.2f <= %.2f)' % (pressure_msg.data, self.object_presence_pressure_threshold)) return False else: if opening_msg.data <= self.object_presence_opening_threshold: rospy.loginfo('Gripper grasp query false: gripper opening is below threshold (%.2f <= %.2f)' % (opening_msg.data, self.object_presence_opening_threshold)) return False else: if (left_pos > 0.85 and right_pos > 1.10) or (right_pos < -0.85 and left_pos < -1.10): rospy.loginfo('Gripper grasp query false: gripper fingers are too off center (%.2f, %.2f)' % (left_pos, right_pos)) return False rospy.loginfo('Gripper grasp query true: pressure is %.2f, opening is %.2f' % (pressure_msg.data, opening_msg.data)) return True
class PickAndPlaceServer(object): def __init__(self): rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Succesfully connected.") rospy.loginfo("Connecting to place AS") self.place_ac = SimpleActionClient('/place', PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Succesfully connected.") self.scene = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy( '/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy( '/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the object size self.object_height = rospy.get_param('~object_height') self.object_width = rospy.get_param('~object_width') self.object_depth = rospy.get_param('~object_depth') # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact") else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer( '/pickup_pose', PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer( '/place_pose', PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start() def pick_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.grasp_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.pick_as.set_aborted(p_res) else: self.pick_as.set_succeeded(p_res) def place_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.place_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.place_as.set_aborted(p_res) else: self.place_as.set_succeeded(p_res) def wait_for_planning_scene_object(self, object_name='part'): rospy.loginfo( "Waiting for object '" + object_name + "'' to appear in planning scene...") gps_req = GetPlanningSceneRequest() gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: # This call takes a while when rgbd sensor is set gps_resp = self.scene_srv.call(gps_req) # check if 'part' is in the answer for collision_obj in gps_resp.scene.world.collision_objects: if collision_obj.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo("'" + object_name + "'' is in scene!") def grasp_object(self, object_pose): rospy.loginfo("Removing any previous 'part' object") self.scene.remove_attached_object("arm_tool_link") self.scene.remove_world_object("part") self.scene.remove_world_object("table") self.scene.remove_world_object("left") self.scene.remove_world_object("right") self.scene.remove_world_object("up") self.scene.remove_world_object("back") rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) # Removing is fast rospy.loginfo("Adding new 'part' object") rospy.loginfo("Object pose: %s", object_pose.pose) #Add object description in scene self.scene.add_box("part", object_pose, (self.object_depth, self.object_width, self.object_height)) rospy.loginfo("Second%s", object_pose.pose) table_pose = copy.deepcopy(object_pose) #define a virtual table below the object table_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8 table_width = 1.8 table_depth = 0.8 table_pose.pose.position.z += -(2*self.object_height)/2 -table_height/2 +0.11 #before also -0.1 and was object width instead of object height table_height -= 0.1 #remove few milimeters to prevent contact between the object and the table self.scene.add_box("table", table_pose, (table_depth, table_width, table_height)) # # We need to wait for the object part to appear self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("table") right_pose = copy.deepcopy(object_pose) #define a virtual table below the object right_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8 right_width = 1.8 right_depth = 0.8 right_pose.pose.position.y += 0.5 #before also -0.1 and was object width instead of object height right_width = 0.2 #remove few milimeters to prevent contact between the object and the table self.scene.add_box("right", right_pose, (right_depth, right_width, right_height)) # # We need to wait for the object part to appear self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("right") left_pose = copy.deepcopy(object_pose) #define a virtual table below the object left_height = 0.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8 left_width = 0.8 left_depth = 0.2 left_pose.pose.position.x += 0.7 #before also -0.1 and was object width instead of object height self.scene.add_box("back", left_pose, (left_depth, left_width, left_height)) self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("back") up_pose = copy.deepcopy(object_pose) #define a virtual table below the object up_height = 0.2 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8 up_width = 1.8 up_depth = 0.8 up_pose.pose.position.z += 0.5 #before also -0.1 and was object width instead of object height self.scene.add_box("up", up_pose, (up_depth, up_width, up_height)) back_pose = copy.deepcopy(object_pose) #define a virtual table below the object back_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8 back_width = 1.8 back_depth = 0.8 back_pose.pose.position.y -= 0.5 #before also -0.1 and was object width instead of object height back_width = 0.2 #remove few milimeters to prevent contact between the object and the table self.scene.add_box("left", back_pose, (back_depth, back_width, back_height)) self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("left") # compute grasps possible_grasps = self.sg.create_grasps_from_object_pose(object_pose) self.pickup_ac goal = createPickupGoal( "arm", "part", object_pose, possible_grasps, self.links_to_allow_contact) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.logdebug("Using torso result: " + str(result)) rospy.loginfo( "Pick result: " + str(moveit_error_dict[result.error_code.val])) return result.error_code.val def place_object(self, object_pose): rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) possible_placings = self.sg.create_placings_from_object_pose( object_pose) # Try only with arm rospy.loginfo("Trying to place using only arm") goal = createPlaceGoal( object_pose, possible_placings, "arm", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo(str(moveit_error_dict[result.error_code.val])) if str(moveit_error_dict[result.error_code.val]) != "SUCCESS": rospy.loginfo( "Trying to place with arm and torso") # Try with arm and torso goal = createPlaceGoal( object_pose, possible_placings, "arm", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.logerr(str(moveit_error_dict[result.error_code.val])) # print result rospy.loginfo( "Result: " + str(moveit_error_dict[result.error_code.val])) rospy.loginfo("Removing previous 'part' object") self.scene.remove_world_object("part") return result.error_code.val
class PathExecutor: goal_index = 0 reached_all_nodes = True def __init__(self): rospy.loginfo('__init__ start') # create a node rospy.init_node(NODE) # Action server to receive goals self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, self.handle_path, auto_start=False) self.path_server.start() # publishers & clients self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path) # get parameters from launch file self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True) # action server based on use_obstacle_avoidance if self.use_obstacle_avoidance == False: self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction) else: self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction) self.goal_client.wait_for_server() # other fields self.goal_index = 0 self.executePathGoal = None self.executePathResult = ExecutePathResult() def handle_path(self, paramExecutePathGoal): ''' Action server callback to handle following path in succession ''' rospy.loginfo('handle_path') self.goal_index = 0 self.executePathGoal = paramExecutePathGoal self.executePathResult = ExecutePathResult() if self.executePathGoal is not None: self.visualization_publisher.publish(self.executePathGoal.path) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): if not self.path_server.is_active(): return if self.path_server.is_preempt_requested(): rospy.loginfo('Preempt requested...') # Stop bug2 self.goal_client.cancel_goal() # Stop path_server self.path_server.set_preempted() return if self.goal_index < len(self.executePathGoal.path.poses): moveto_goal = MoveToGoal() moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index] self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal, feedback_cb=self.handle_goal_preempt) # Wait for result while self.goal_client.get_result() is None: if self.path_server.is_preempt_requested(): self.goal_client.cancel_goal() else: rospy.loginfo('Done processing goals') self.goal_client.cancel_goal() self.path_server.set_succeeded(self.executePathResult, 'Done processing goals') return rate.sleep() self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.') def handle_goal(self, state, result): ''' Handle goal events (succeeded, preempted, aborted, unreachable, ...) Send feedback message ''' feedback = ExecutePathFeedback() feedback.pose = self.executePathGoal.path.poses[self.goal_index] # state is GoalStatus message as shown here: # http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html if state == GoalStatus.SUCCEEDED: rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1) self.executePathResult.visited.append(True) feedback.reached = True else: rospy.loginfo("Failed finding goal %d", self.goal_index + 1) self.executePathResult.visited.append(False) feedback.reached = False if not self.executePathGoal.skip_unreachable: rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1) # Stop bug2 self.goal_client.cancel_goal() # Stop path_server self.path_server.set_succeeded(self.executePathResult, 'Unreachable goal in path. Done processing goals.') #self.path_server.set_preempted() #return self.path_server.publish_feedback(feedback) self.goal_index = self.goal_index + 1 def handle_goal_preempt(self, state): ''' Callback for goal_client to check for preemption from path_server ''' if self.path_server.is_preempt_requested(): self.goal_client.cancel_goal()
class BakerArmServer(script): DOF = 5 DEFAULT_POSITION = [0.6, 1., -2., 1., 0.] TRANSPORT_POSITION = [0.9, 1., -2., 1., 0.] IS_GRIPPER_AVAILABLE = False def __init__(self, name, status=ArmStatus.NO_TRASHCAN): self.verbose_ = True self.confirm_ = False self.name_ = name self.status_ = status self.joint_values_ = [0.0] * self.DOF self.mutex_ = Lock() (self.trashcan_pose_, self.trolley_pose_) = (None, None) self.displayParameters() self.initClients() self.initServices() self.initServers() self.initSubscribers() self.initTopics() def confirm(self): if not self.confirm_: return raw_input("Please press enter to confirm") def displayParameters(self): if not self.verbose_: return print("========== baker_arm_server Parameters ==========") print("todo") @log def Initialize(self): self.sss.init('gripper') self.sss.init('arm') @log def initClients(self): # Warning: wait_for_server and wait_for_service are not the same function. # * rospy.duration expected for wait_for_server # * float expected for wait_for_service self.plan_client_ = SimpleActionClient('/arm_planning_node/PlanTo', PlanToAction) self.plan_client_.wait_for_server(timeout=rospy.Duration(5.0)) self.execution_client_ = SimpleActionClient('/traj_exec', ExecuteTrajectoryAction) self.execution_client_.wait_for_server(timeout=rospy.Duration(5.0)) self.small_gripper_finger_client_ = rospy.ServiceProxy( "/gripper/driver/set_object", SetObject) # rmb-ma. keep commented # self.small_gripper_finger_client_.wait_for_service(timeout=5.0) self.add_collision_object_client_ = rospy.ServiceProxy( "/ipa_planning_scene_creator/add_collision_objects", AddCollisionObject) self.add_collision_object_client_.wait_for_service(timeout=5.0) self.attach_object_client_ = rospy.ServiceProxy( "ipa_planning_scene_creator/attach_object", AddCollisionObject) self.attach_object_client_.wait_for_service(timeout=5.0) self.detach_object_client_ = rospy.ServiceProxy( "ipa_planning_scene_creator/detach_object", RemoveCollisionObject) self.detach_object_client_.wait_for_service(timeout=5.0) @log def initServers(self): self.to_joints_position_server_ = SimpleActionServer( self.name_ + '/set_joints_values', ExecuteTrajectoryAction, self.moveToJointsPositionCallback, False) self.to_joints_position_server_.start() self.to_transport_position_server_ = SimpleActionServer( self.name_ + '/transport_position', MoveToAction, self.moveToTransportPositionCallback, False) self.to_transport_position_server_.start() self.to_rest_position_server_ = SimpleActionServer( self.name_ + '/rest_position', MoveToAction, self.moveToRestPositionCallback, False) self.to_rest_position_server_.start() self.catch_trashcan_server_ = SimpleActionServer( self.name_ + '/catch_trashcan', MoveToAction, self.catchTrashcanCallback, False) self.catch_trashcan_server_.start() self.leave_trashcan_server_ = SimpleActionServer( self.name_ + '/leave_trashcan', MoveToAction, self.leaveTrashcanCallback, False) self.leave_trashcan_server_.start() self.empty_trashcan_server_ = SimpleActionServer( self.name_ + '/empty_trashcan', MoveToAction, self.emptyTrashcanCallback, False) self.empty_trashcan_server_.start() @log def initServices(self): pass @log def initTopics(self): Thread(target=self.statusTalker).start() @log def statusTalker(self): publisher = rospy.Publisher(self.name_ + '/status', Int32, queue_size=10) rate = rospy.Rate(5) while not rospy.is_shutdown(): publisher.publish(self.status_.value) rate.sleep() @log def initSubscribers(self): rospy.Subscriber("/arm/joint_states", JointState, self.jointStateCallback) def jointStateCallback(self, msg): ''' getting current position of the joint @param msg containts joint position, velocity, effort, names ''' for i in range(0, len(msg.position)): self.joint_values_[i] = msg.position[i] def gripperHandler(self, finger_value=0.01, finger_open=False, finger_close=True): ''' Gripper handler function use to control gripper open/close command, move parallel part of gripper with give values (in cm) @param finger_value is the big parallel part move realtive to zero position @finger_open open the small finger @finger_close close the small finger ''' if finger_open or finger_close: # set_object request, both the finger move together (at the same time) request = SetObjectRequest() response = SetObjectResponse() request.node = 'gripper_finger1_joint' request.object = '22A0' if finger_open: request.value = '5' elif finger_close: request.value = '10' request.cached = False response = self.small_gripper_finger_client_.call(request) if response.success: rospy.loginfo(response.message) else: rospy.logerr(response.message) rospy.logerr("SetObject service call Failed!!") # default blocking is true, wait until if finishes it tasks self.sss.move('gripper', [[finger_value]]) return response.success @log def handleCheckAccessibility(self, request): response = TriggerResponse() response.success = True return response @log def openGripper(self): if not self.IS_GRIPPER_AVAILABLE: return self.gripperHandler(finger_value=0.01, finger_open=True, finger_close=False) @log def closeGripper(self): if not self.IS_GRIPPER_AVAILABLE: return self.gripperHandler(finger_value=-0.01, finger_open=False, finger_close=True) @log def executeTrajectory(self, trajectory, server): goal = ExecuteTrajectoryGoal() goal.trajectory = trajectory self.execution_client_.send_goal(goal) while self.execution_client_.get_state() < 3: rospy.sleep(0.1) if not server.is_preempt_requested() and not rospy.is_shutdown(): continue self.execution_client_.cancel_goal() self.execution_client_.wait_for_result() return True result = self.execution_client_.get_result() state = self.execution_client_.get_state() if result.success and state == 3: rospy.logwarn("Execution finish with given time") return True else: rospy.logerr("Execution aborted!!") return False @log def isPoseAccessible(self, target_pose): (_, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_, object_pose=target_pose, bdmp_goal=None, bdmp_action='') return is_planned @log def planAndExecuteTrajectoryInJointSpaces(self, target_joints, server): (trajectory, is_planned) = planTrajectoryInJointSpace(client=self.plan_client_, object_pose=target_joints, bdmp_goal=None, bdmp_action='') if not is_planned: rospy.logerr('Trajectory in joint spaces execution failed') raise Exception('Trajectory in joint spaces execution failed') self.confirm() is_execution_successful = self.executeTrajectory(trajectory=trajectory, server=server) if not is_execution_successful: rospy.logerr('Can not find valid trajectory at joint space') raise Exception('Trajectory Execution failed') return not server.is_preempt_requested() @log def planAndExecuteTrajectoryInCartesianSpace(self, target_pose, server): (trajectory, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_, object_pose=target_pose, bdmp_goal=None, bdmp_action='') if not is_planned: rospy.logerr('Cannot find valid trajectory at cartesianSpace') raise Exception('Cannot find valid trajectory at cartesian space') self.confirm() is_execution_successful = self.executeTrajectory(trajectory=trajectory, server=server) if not is_execution_successful: rospy.logerr("Trajectory Execution Failed") raise Exception('Trajectory Execution Failed') return not server.is_preempt_requested() @staticmethod def poseToLists(pose): position = [pose.position.x, pose.position.y, pose.position.z] orientation = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] return (position, orientation) @log def catchTrashcanCallback(self, goal): rospy.sleep(3) result = MoveToResult() result.arrived = False target_pose = goal.target_pos target_pose.pose.position.z *= 1.1 #The arm canot carry a new trashcan as it already carries one if self.status_ != ArmStatus.NO_TRASHCAN: self.catch_trashcan_server_.set_aborted(result) try: self.planAndExecuteTrajectoryInCartesianSpace( target_pose, self.catch_trashcan_server_) except Exception as e: print(e) self.catch_trashcan_server_.set_aborted(result) return if self.catch_trashcan_server_.is_preempt_requested(): result.arrived = True self.catch_trashcan_server_.set_preempted(result) return self.closeGripper() #self.fixTrashCanToRobot() self.status_ = ArmStatus.FULL_TRASHCAN # todo rmb-ma setup the grapping move with a real trashcan # Help: # try: # target_pose = make_pose(position=[0.00,-0.00,-0.07], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper') # self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_) # except: # self.catch_trashcan_server_.set_aborted(result) # return # # if self.catch_trashcan_server_.is_preempt_requested(): # result.arrived = True # self.catch_trashcan_server_.set_preempted(result) # return # # try: # target_pose = make_pose(position=[0.020,-0.00,0.00], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper') # self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_) # except: # self.catch_trashcan_server_.set_aborted(result) # return # # if self.catch_trashcan_server_.is_preempt_requested(): # result.arrived = True # self.catch_trashcan_server_.set_preempted(result) # return # # try: # target_pose = make_pose(position=[0.000,-0.00,0.030], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper') # self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_) # except: # self.catch_trashcan_server_.set_aborted(result) # return # result.arrived = True self.catch_trashcan_server_.set_succeeded(result) @log def emptyTrashcanCallback(self, goal): result = MoveToResult() #Trashcan emptying while the arm doesnt carry any trashcan if self.status_ != ArmStatus.FULL_TRASHCAN: result.arrived = False self.empty_trashcan_server_.set_aborted(result) return try: self.planAndExecuteTrajectoryInCartesianSpace( goal.target_pos, self.empty_trashcan_server_) except: result.arrived = False self.empty_trashcan_server_.set_aborted(result) return if self.empty_trashcan_server_.is_preempt_requested(): self.empty_trashcan_server_.set_preempted() return try: target_joints = self.joint_values_[0:-1] + [3.] self.planAndExecuteTrajectoryInJointSpaces( target_joints, self.empty_trashcan_server_) except: result.arrived = False self.empty_trashcan_server_.set_aborted(result) return if self.empty_trashcan_server_.is_preempt_requested(): self.empty_trashcan_server_.set_preempted() return rospy.sleep(5) self.status_ = ArmStatus.FULL_TRASHCAN target_joints = self.joint_values_[0:-1] + [0] self.planAndExecuteTrajectoryInJointSpaces(target_joints, self.empty_trashcan_server_) result.arrived = True self.empty_trashcan_server_.set_succeeded(result) @log def moveToRestPositionCallback(self, goal): result = MoveToResult() # Rest position is unavailable if the arm carries a trashcan if self.status_ != ArmStatus.NO_TRASHCAN: result.arrived = False self.to_rest_position_server_.set_aborted(result) return try: self.planAndExecuteTrajectoryInJointSpaces( self.DEFAULT_POSITION, self.to_rest_position_server_) except Exception as e: rospy.logerr(e) result.arrived = False self.to_rest_position_server_.set_aborted(result) return if self.to_rest_position_server_.is_preempt_requested(): self.to_rest_position_server_.set_preempted() return self.to_rest_position_server_.set_succeeded() @log def moveToTransportPositionCallback(self, goal): result = MoveToResult() # Transport position is unavailable if the arm doesnt carry a trashcan if self.status_ == ArmStatus.NO_TRASHCAN: result.arrived = False self.to_transport_position_server_.set_aborted(result) return try: result.arrived = self.planAndExecuteTrajectoryInJointSpaces( self.TRANSPORT_POSITION, self.to_transport_position_server_) except: result.arrived = False self.to_transport_position_server_.set_aborted(result) return if self.to_transport_position_server_.is_preempt_requested(): self.to_transport_position_server_.set_preempted() return self.to_transport_position_server_.set_succeeded(result) @log def leaveTrashcanCallback(self, goal): result = MoveToResult() # The arm cannot leave a trashcan if it doesnt carry one if self.status_ != ArmStatus.EMPTY_TRASHCAN: result.arrived = False self.leave_trashcan_server_.set_aborted(result) return self.planAndExecuteTrajectoryInCartesianSpace( goal.target_pos, self.leave_trashcan_server_) if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return self.openGripper() self.status_ = ArmStatus.NO_TRASHCAN if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return target_pose = make_pose(position=[-0.07, -0., -0.04], orientation=[0., 0., 0., 1.], frame_id='gripper') self.planAndExecuteTrajectoryInCartesianSpace( target_pose, self.leave_trashcan_server_) if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return target_pose = make_pose(position=[-0.0, -0., 0.05], orientation=[0., 0., 0., 1.], frame_id='gripper') self.planAndExecuteTrajectoryInCartesianSpace( target_pose, self.leave_trashcan_server_) if self.leave_trashcan_server_.is_preempt_requested(): self.leave_trashcan_server_.set_preempted() return result.arrived = True self.leave_trashcan_server_.set_succeeded(result) @log def moveToJointsPositionCallback(self, goal): target_joints = goal.trajectory.joint_trajectory.points[0].positions self.planAndExecuteTrajectoryInJointSpaces( target_joints, self.to_joints_position_server_) if self.to_joints_position_server_.is_preempt_requested(): self.to_joints_position_server_.set_preempted() return self.to_joints_position_server_.set_succeeded()
class GraspObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning) self.get_solver_info_srv = rospy.ServiceProxy('/wubble2_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo) self.get_ik_srv = rospy.ServiceProxy('/wubble2_left_arm_kinematics/get_ik', GetPositionIK) self.set_planning_scene_diff_client = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, GraspObjectAction, execute_cb=self.grasp_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME) rospy.wait_for_service('/GraspPlanning') rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik_solver_info') rospy.loginfo('%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik') rospy.loginfo('%s: connected to wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME) rospy.loginfo('%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME) rospy.wait_for_service('/environment_server/set_planning_scene_diff') rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) self.action_server.start() def find_ik_for_grasping_pose(self, pose_stamped): solver_info = self.get_solver_info_srv() arm_joints = solver_info.kinematic_solver_info.joint_names req = GetPositionIKRequest() req.timeout = rospy.Duration(5.0) req.ik_request.ik_link_name = GRIPPER_LINK_FRAME req.ik_request.pose_stamped = pose_stamped try: current_state = rospy.wait_for_message('/joint_states', JointState, 2.0) req.ik_request.ik_seed_state.joint_state.name = arm_joints req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints] if not self.set_planning_scene_diff_client(): rospy.logerr('%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME) return None ik_result = self.get_ik_srv(req) if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS: return ik_result.solution else: rospy.logerr('%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME) return None except: rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME) return None def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''): """ target = GraspableObject collision_object_name = name of target in collision map collision_support_surface_name = name of surface target is touching """ req = GraspPlanningRequest() req.arm_name = ARM_GROUP_NAME req.target = target req.collision_object_name = collision_object_name req.collision_support_surface_name = collision_support_surface_name rospy.loginfo('%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name)) grasping_result = self.grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME) return None pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME) return self.find_ik_for_grasping_pose(pose_stamped) def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() rospy.sleep(1) grasp_status = self.get_grasp_status_srv() return grasp_status.is_hand_occupied def grasp_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() target = goal.graspable_object collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation1 = CollisionOperation() collision_operation1.object1 = collision_object_name collision_operation1.object2 = GRIPPER_GROUP_NAME collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE # collision_operation2.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2] # set gripper padding to 0 gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) self.open_gripper() # move into pre-grasp pose if not move_arm_joint_goal(self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record grasping sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) grasp_successful = self.close_gripper() rospy.sleep(0.5) # if grasp was successful, attach it to the gripper if grasp_successful: sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded(GraspObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted()
class OnlineBagger(object): BAG_TOPIC = '/online_bagger/bag' def __init__(self): """ Make dictionary of dequeues. Subscribe to set of topics defined by the yaml file in directory Stream topics up to a given stream time, dump oldest messages when limit is reached Set up service to bag n seconds of data default to all of available data """ self.successful_subscription_count = 0 # successful subscriptions self.iteration_count = 0 # number of iterations self.streaming = True self.get_params() if len(self.subscriber_list) == 0: rospy.logwarn('No topics selected to subscribe to. Closing.') rospy.signal_shutdown('No topics to subscribe to') return self.make_dicts() self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC, BagOnlineAction, execute_cb=self.start_bagging, auto_start=False) self.subscribe_loop() rospy.loginfo('Remaining Failed Topics: {}\n'.format( self.get_subscriber_list(False))) self._action_server.start() def get_subscriber_list(self, status): """ Get string of all topics, if their subscribe status matches the input (True / False) Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string) """ sub_list = '' for topic in self.subscriber_list.keys(): if self.subscriber_list[topic][1] == status: sub_list = sub_list + \ '\n{:13}, {}'.format(self.subscriber_list[topic], topic) return sub_list def get_params(self): """ Retrieve parameters from param server. """ self.dir = rospy.get_param('~bag_package_path', default=None) # Handle bag directory for MIL bag script if self.dir is None and 'BAG_DIR' in os.environ: self.dir = os.environ['BAG_DIR'] self.stream_time = rospy.get_param( '~stream_time', default=30) # seconds self.resubscribe_period = rospy.get_param( '~resubscribe_period', default=3.0) # seconds self.dated_folder = rospy.get_param( '~dated_folder', default=True) # bool self.subscriber_list = {} topics_param = rospy.get_param('~topics', default=[]) # Add topics from rosparam to subscribe list for topic in topics_param: time = topic[1] if len(topic) == 2 else self.stream_time self.subscriber_list[topic[0]] = (time, False) def add_unique_topic(topic): if topic not in self.subscriber_list: self.subscriber_list[topic] = (self.stream_time, False) def add_env_var(var): for topic in var.split(): add_unique_topic(topic) # Add topics from MIL bag script environment variables if 'BAG_ALWAYS' in os.environ: add_env_var(os.environ['BAG_ALWAYS']) for key in os.environ.keys(): if key[0:4] == 'bag_': add_env_var(os.environ[key]) rospy.loginfo( 'Default stream_time: {} seconds'.format(self.stream_time)) rospy.loginfo('Bag Directory: {}'.format(self.dir)) def make_dicts(self): """ Make dictionary with sliceable deques() that will be filled with messages and time stamps. Subscriber list contains all of the topics, their stream time and their subscription status: A True status for a given topic corresponds to a successful subscription A False status indicates a failed subscription. Stream time for an individual topic is specified in seconds. For Example: self.subscriber_list[0:1] = [['/odom', 300 ,False], ['/absodom', 300, True]] Indicates that '/odom' has not been subscribed to, but '/absodom' has been subscribed to self.topic_messages is a dictionary of deques containing a list of tuples. Dictionary Keys contain topic names Each value for each topic contains a deque Each deque contains a list of tuples Each tuple contains a message and its associated time stamp For example: '/odom' is a potential topic name self.topic_message['/odom'] is a deque self.topic_message['/odom'][0] is the oldest message available in the deque and its time stamp if available. It is a tuple with each element: (time_stamp, msg) self.topic_message['/odom'][0][0] is the time stamp for the oldest message self.topic_message['/odom'][0][1] is the message associated with the oldest topic """ self.topic_messages = {} class SliceableDeque(deque): def __getitem__(self, index): if isinstance(index, slice): return type(self)(itertools.islice(self, index.start, index.stop, index.step)) return deque.__getitem__(self, index) for topic in self.subscriber_list: self.topic_messages[topic] = SliceableDeque(deque()) rospy.loginfo('Initial subscriber_list: {}'.format( self.get_subscriber_list(False))) def subscribe_loop(self): """ Continue to subscribe until at least one topic is successful, then break out of loop and be called in the callback function to prevent the function from locking up. """ self.resubscriber = None i = 0 # if self.successful_subscription_count == 0 and not # rospy.is_shutdown(): while self.successful_subscription_count == 0 and not rospy.is_shutdown(): self.subscribe() rospy.sleep(0.1) i = i + 1 if i % 1000 == 0: rospy.logdebug('still subscribing!') rospy.loginfo("Subscribed to {} of {} topics, will try again every {} seconds".format( self.successful_subscription_count, len(self.subscriber_list), self.resubscribe_period)) self.resubscriber = rospy.Timer(rospy.Duration( self.resubscribe_period), self.subscribe) def subscribe(self, time_info=None): """ Subscribe to the topics defined in the yaml configuration file Function checks subscription status True/False of each topic if True: topic has already been sucessfully subscribed to if False: topic still needs to be subscribed to and subscriber will be run. Each element in self.subscriber list is a list [topic, Bool] where the Bool tells the current status of the subscriber (sucess/failure). Return number of topics that failed subscription """ if self.successful_subscription_count == len(self.subscriber_list): if self.resubscriber is not None: self.resubscriber.shutdown() rospy.loginfo( 'All topics subscribed too! Shutting down resubscriber') for topic, (time, subscribed) in self.subscriber_list.items(): if not subscribed: msg_class = rostopic.get_topic_class(topic) if msg_class[1] is not None: self.successful_subscription_count += 1 rospy.Subscriber(topic, msg_class[0], lambda msg, _topic=topic: self.bagger_callback(msg, _topic)) self.subscriber_list[topic] = (time, True) def get_topic_duration(self, topic): """ Return current time duration of topic """ return self.topic_messages[topic][-1][0] - self.topic_messages[topic][0][0] def get_header_time(self, msg): """ Retrieve header time if available """ if hasattr(msg, 'header'): return msg.header.stamp else: return rospy.get_rostime() def get_time_index(self, topic, requested_seconds): """ Return the index for the time index for a topic at 'n' seconds from the end of the dequeue For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most recent message can be obtained with this function. The number of requested seconds should be the number of seoncds desired from the end of deque. (ie. requested_seconds = 10 ) If the desired time length of the bag is greater than the available messages it will output a message and return how ever many seconds of data are avaiable at the moment. Seconds is of a number type (not a rospy.Time type) (ie. int, float) """ topic_duration = self.get_topic_duration(topic).to_sec() if topic_duration == 0: return 0 ratio = requested_seconds / topic_duration index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1))) return index def bagger_callback(self, msg, topic): """ Streaming callback function, stops streaming during bagging process also pops off msgs from dequeue if stream size is greater than specified stream_time Stream, callback function does nothing if streaming is not active """ if not self.streaming: return self.iteration_count = self.iteration_count + 1 time = self.get_header_time(msg) self.topic_messages[topic].append((time, msg)) time_diff = self.get_topic_duration(topic) # verify streaming is popping off and recording topics if self.iteration_count % 100 == 0: rospy.logdebug("{} has {} messages spanning {} seconds".format( topic, self.get_topic_message_count(topic), round(time_diff.to_sec(), 2))) while time_diff > rospy.Duration(self.subscriber_list[topic][0]) and not rospy.is_shutdown(): self.topic_messages[topic].popleft() time_diff = self.get_topic_duration(topic) def get_topic_message_count(self, topic): """ Return number of messages available in a topic """ return len(self.topic_messages[topic]) def get_total_message_count(self): """ Returns total number of messages across all topics """ total_message_count = 0 for topic in self.topic_messages.keys(): total_message_count = total_message_count + \ self.get_topic_message_count(topic) return total_message_count def _get_default_filename(self): return str(datetime.date.today()) + '-' + str(datetime.datetime.now().time())[0:8] def get_bag_name(self, filename=''): """ Create ros bag save directory If no bag name is provided, the current date/time is used as default. """ # If directory param is not set, default to $HOME/bags/<date> default_dir = self.dir if default_dir is None: default_dir = os.path.join(os.environ['HOME'], 'bags') # if dated folder param is set to True, append current date to # directory if self.dated_folder is True: default_dir = os.path.join(default_dir, str(datetime.date.today())) # Split filename from directory bag_dir, bag_name = os.path.split(filename) bag_dir = os.path.join(default_dir, bag_dir) if not os.path.exists(bag_dir): os.makedirs(bag_dir) # Create default filename if only directory specified if bag_name == '': bag_name = self._get_default_filename() # Make sure filename ends in .bag, add otherwise if bag_name[-4:] != '.bag': bag_name = bag_name + '.bag' return os.path.join(bag_dir, bag_name) def start_bagging(self, req): """ Dump all data in dictionary to bags, temporarily stops streaming during the bagging process, resumes streaming when over. If bagging is already false because of an active call to this service """ result = BagOnlineResult() if self.streaming is False: result.status = 'Bag Request came in while bagging, priority given to prior request' result.success = False self._action_server.set_aborted(result) return bag = None try: self.streaming = False result.filename = self.get_bag_name(req.bag_name) bag = rosbag.Bag(result.filename, 'w') requested_seconds = req.bag_time selected_topics = req.topics.split() feedback = BagOnlineFeedback() total_messages = 0 bag_topics = {} for topic, (time, subscribed) in self.subscriber_list.iteritems(): if not subscribed: continue # Exclude topics that aren't in topics service argument # If topics argument is empty string, include all topics if len(selected_topics) > 0 and topic not in selected_topics: continue if len(self.topic_messages[topic]) == 0: continue if req.bag_time == 0: index = 0 else: index = self.get_time_index(topic, requested_seconds) total_messages += len(self.topic_messages[topic][index:]) bag_topics[topic] = index if total_messages == 0: result.success = False result.status = 'no messages' self._action_server.set_aborted(result) self.streaming = True bag.close() return self._action_server.publish_feedback(feedback) msg_inc = 0 for topic, index in bag_topics.iteritems(): for msgs in self.topic_messages[topic][index:]: bag.write(topic, msgs[1], t=msgs[0]) if msg_inc % 50 == 0: # send feedback every 50 messages feedback.progress = float(msg_inc) / total_messages self._action_server.publish_feedback(feedback) msg_inc += 1 # empty deque when done writing to bag self.topic_messages[topic].clear() feedback.progress = 1.0 self._action_server.publish_feedback(feedback) bag.close() except Exception as e: result.success = False result.status = 'Exception while writing bag: ' + str(e) self._action_server.set_aborted(result) self.streaming = True if bag is not None: bag.close() return rospy.loginfo('Bag written to {}'.format(result.filename)) result.success = True self._action_server.set_succeeded(result) self.streaming = True
class DropObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, DropObjectAction, execute_cb=self.drop_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def drop_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() # check that we have something in hand before dropping it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME) self.action_server.set_aborted() return # record sound padded by 0.5 seconds from start and 1.5 seconds from the end self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(1.5) # check if gripper actually opened first sound_msg = None grasp_status = self.get_grasp_status_srv() # if there something in the gripper - drop failed if grasp_status.is_hand_occupied: self.stop_audio_recording_srv(False) else: sound_msg = self.stop_audio_recording_srv(True) # delete the object that we just dropped, we don't really know where it will land obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.REMOVE obj.object.id = goal.collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded(DropObjectResult(sound_msg.recorded_sound)) else: self.action_server.set_aborted()
class Kill(): REFRESH_RATE = 20 def __init__(self): self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482] self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482] self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675] self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919] self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919] self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644] self.v = [0] * len(self.r1) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self._action_name = 'kill' self._tf = tf.TransformListener() #initialize base controller topic_name = '/base_controller/command' self._base_publisher = rospy.Publisher(topic_name, Twist) #Initialize the sound controller self._sound_client = SoundClient() # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.pose = None self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback) #initialize this client self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def marker_callback(self, marker): if (self.pose is not None): self.pose = marker.pose def run(self, goal): rospy.loginfo('Begin kill') self.pose = goal.pose #pose = self._tf.transformPose('/base_link', goal.pose) self._sound_client.say('Halt!') # turn to face the marker before opening arms (code repeated later) r = rospy.Rate(self.REFRESH_RATE) while(True): pose = self.pose if abs(pose.pose.position.y) > .1: num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / ( 3 * abs(pose.pose.position.y))) for i in range(num_move_y): if pose != self.pose: break self._base_publisher.publish(twist_msg) r.sleep() pose.pose.position.y = 0 else: break # open arms traj_goal_r = JointTrajectoryGoal() traj_goal_r.trajectory.joint_names = self.r_joint_names traj_goal_l = JointTrajectoryGoal() traj_goal_l.trajectory.joint_names = self.l_joint_names traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2))) self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3)) traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2))) self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3)) traj_goal_r.trajectory.points[0].positions = self.r2 self.r_traj_action_client.send_goal(traj_goal_r) traj_goal_l.trajectory.points[0].positions = self.l2 self.l_traj_action_client.send_goal(traj_goal_l) self._sound_client.say('You have the right to remain silent.') # Keep a local copy because it will update #pose = self.pose #num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) #twist_msg = Twist() #twist_msg.linear = Vector3(.1, 0.0, 0.0) #twist_msg.angular = Vector3(0.0, 0.0, 0.0) #for i in range(num_move_x): # self._base_publisher.publish(twist_msg) # r.sleep() # track the marker as much as we can while True: pose = self.pose # too far away if abs(pose.pose.position.x > 1.5): rospy.loginfo('Target out of range: ' + str(pose.pose.position.x)) self._as.set_aborted() return; # too far to the side -> rotate elif abs(pose.pose.position.y) > .1: num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y))) for i in range(num_move_y): if pose != self.pose: break self._base_publisher.publish(twist_msg) r.sleep() pose.pose.position.y = 0 #twist_msg = Twist() #twist_msg.linear = Vector3(0.0, 0.0, 0.0) #twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y))) #self._base_publisher.publish(twist_msg) # now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over) elif pose.pose.position.x > .5: num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1 #print str(pose.pose.position.x) + ', ' + str(num_move_x) twist_msg = Twist() twist_msg.linear = Vector3(.1, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for i in range(num_move_x): if pose != self.pose: break self._base_publisher.publish(twist_msg) r.sleep() pose.pose.position.x = 0 #twist_msg = Twist() #twist_msg.linear = Vector3(.1, 0.0, 0.0) #twist_msg.angular = Vector3(0.0, 0.0, 0.0) #self._base_publisher.publish(twist_msg) # susupect is within hugging range!!! else: break r.sleep() # after exiting the loop, we should be ready to capture -> send close arms goal self._sound_client.say("Anything you say do can and will be used against you in a court of law.") self.l_traj_action_client.wait_for_result(rospy.Duration(3)) self.r_traj_action_client.wait_for_result(rospy.Duration(3)) traj_goal_r.trajectory.points[0].positions = self.r3 self.r_traj_action_client.send_goal(traj_goal_r) traj_goal_l.trajectory.points[0].positions = self.l3 self.l_traj_action_client.send_goal(traj_goal_l) self.l_traj_action_client.wait_for_result(rospy.Duration(3)) self.r_traj_action_client.wait_for_result(rospy.Duration(3)) rospy.loginfo('End kill') rospy.sleep(20) self._as.set_succeeded()
class PlaceObjectActionServer: def __init__(self): self.start_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy( '/audio_dump/stop_audio_recording', StopAudioRecording) self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction) self.attached_object_pub = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject) self.action_server = SimpleActionServer(ACTION_NAME, PlaceObjectAction, execute_cb=self.place_object, auto_start=False) def initialize(self): rospy.loginfo( '%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo( '%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo( '%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo( '%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME) self.posture_controller.wait_for_server() rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME) rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME) self.move_arm_client.wait_for_server() rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME) self.action_server.start() def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2, collision_operation3 ] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded( PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
class Approach(object): def __init__(self, name, takeoff_height): self.robot_name = name self.takeoff_height = takeoff_height # Mutual exclusion self.sonar_me = Condition() self.odometry_me = Condition() self.current_height = None # Create trajectory server self.trajectory_server = SimpleActionServer( 'approach_server', ExecuteDroneApproachAction, self.goCallback, False) self.server_feedback = ExecuteDroneApproachFeedback() self.server_result = ExecuteDroneApproachResult() # Get client from hector_quadrotor_actions self.move_client = SimpleActionClient("/{}/action/pose".format(name), PoseAction) self.move_client.wait_for_server() # Subscribe to ground_truth to monitor the current pose of the robot rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry, self.poseCallback) # Subscribe to topic to receive the planned trajectory rospy.Subscriber("/{}/move_group/display_planned_path".format(name), DisplayTrajectory, self.planCallback) #Auxiliary variables self.trajectory = [] # Array with the trajectory to be executed self.trajectory_received = False # Flag to signal trajectory received self.odom_received = False # Flag to signal odom received self.robot = RobotCommander( robot_description="{}/robot_description".format(name), ns="/{}".format(name)) self.display_trajectory_publisher = rospy.Publisher( '/{}/move_group/display_planned_path'.format(name), DisplayTrajectory, queue_size=20) # Variables for collision callback self.validity_srv = rospy.ServiceProxy( '/{}/check_state_validity'.format(name), GetStateValidity) self.validity_srv.wait_for_service() self.collision = False # Subscribe to sonar_height rospy.Subscriber("sonar_height", Range, self.sonar_callback, queue_size=10) #Start move_group self.move_group = MoveGroup('earth', name) self.move_group.set_planner(planner_id='RRTConnectkConfigDefault', attempts=10, allowed_time=2) #RRTConnectkConfigDefault self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX, ZMAX]) # Set the workspace size # Get current robot position to define as start planning point self.current_pose = self.robot.get_current_state() #Start planningScenePublisher self.scene_pub = PlanningScenePublisher(name, self.current_pose) # Start trajectory server self.trajectory_server.start() def sonar_callback(self, msg): ''' Function to update drone height ''' self.sonar_me.acquire() self.current_height = msg.range self.sonar_me.notify() self.sonar_me.release() def poseCallback(self, odometry): ''' Monitor the current position of the robot ''' self.odometry_me.acquire() self.odometry = odometry.pose.pose # print(self.odometry) self.odometry_me.release() self.odom_received = True def goCallback(self, pose): ''' Require a plan to go to the desired target and try to execute it 5 time or return erro ''' self.target = pose.goal #################################################################### # First takeoff if the drone is close to ground self.sonar_me.acquire() while not (self.current_height and self.odometry): self.sonar_me.wait() if self.current_height < self.takeoff_height: self.odometry_me.acquire() takeoff_pose = PoseGoal() takeoff_pose.target_pose.header.frame_id = "{}/world".format( self.robot_name) takeoff_pose.target_pose.pose.position.x = self.odometry.position.x takeoff_pose.target_pose.pose.position.y = self.odometry.position.y takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w self.odometry_me.release() self.move_client.send_goal(takeoff_pose) self.move_client.wait_for_result() result = self.move_client.get_state() if result == GoalStatus.ABORTED: rospy.logerr("Abort approach! Unable to execute takeoff!") self.trajectory_server.set_aborted() return self.sonar_me.release() #################################################################### rospy.loginfo("Try to start from [{},{},{}]".format( self.odometry.position.x, self.odometry.position.y, self.odometry.position.z)) rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x, self.target.position.y, self.target.position.z)) self.trials = 0 while self.trials < 5: rospy.logwarn("Attempt {}".format(self.trials + 1)) if (self.trials > 1): self.target.position.z += 2 * rd() - 1 result = self.go(self.target) if (result == 'replan') or (result == 'no_plan'): self.trials += 1 else: self.trials = 10 self.collision = False if result == 'ok': self.trajectory_server.set_succeeded() elif (result == 'preempted'): self.trajectory_server.set_preempted() else: self.trajectory_server.set_aborted() def go(self, target_): ''' Function to plan and execute the trajectory one time ''' # Insert goal position on an array target = [] target.append(target_.position.x) target.append(target_.position.y) target.append(target_.position.z) target.append(target_.orientation.x) target.append(target_.orientation.y) target.append(target_.orientation.z) target.append(target_.orientation.w) #Define target for move_group # self.move_group.set_joint_value_target(target) self.move_group.set_target(target) self.odometry_me.acquire() self.current_pose.multi_dof_joint_state.transforms[ 0].translation.x = self.odometry.position.x self.current_pose.multi_dof_joint_state.transforms[ 0].translation.y = self.odometry.position.y self.current_pose.multi_dof_joint_state.transforms[ 0].translation.z = self.odometry.position.z self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.x self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.y self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.z self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.w self.odometry_me.release() #Set start state self.move_group.set_start_state(self.current_pose) # Plan a trajectory till the desired target plan = self.move_group.plan() if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory while (not self.trajectory_received): rospy.loginfo("Waiting for trajectory!") rospy.sleep(0.2) # rospy.loginfo("TRAJECTORY: {}".format(self.trajectory)) #Execute trajectory with action_pose last_pose = self.trajectory[0] for pose in self.trajectory: # Verify preempt call if self.trajectory_server.is_preempt_requested(): self.move_client.send_goal(last_pose) self.move_client.wait_for_result() self.scene_pub.publishScene() self.trajectory_received = False self.odom_received = False return 'preempted' #Send next pose to move self.next_pose = pose.target_pose.pose self.move_client.send_goal(pose, feedback_cb=self.collisionCallback) self.move_client.wait_for_result() result = self.move_client.get_state() self.scene_pub.publishScene() # Abort if the drone can not reach the position if result == GoalStatus.ABORTED: self.move_client.send_goal( last_pose) #Go back to the last pose self.move_client.wait_for_result() self.trajectory_received = False self.odom_received = False return 'aborted' elif result == GoalStatus.PREEMPTED: # last_pose.target_pose.pose.position.z += rd() - 0.5 self.move_client.send_goal( last_pose) #Go back to the last pose self.move_client.wait_for_result() self.trajectory_received = False self.odom_received = False return 'replan' elif result == GoalStatus.SUCCEEDED: self.trials = 0 last_pose = pose self.server_feedback.current_pose = self.odometry self.trajectory_server.publish_feedback(self.server_feedback) # Reset control variables self.trajectory_received = False self.odom_received = False rospy.loginfo("Trajectory is traversed!") return 'ok' else: rospy.logerr("Trajectory is empty. Planning was unsuccessful.") return 'no_plan' def planCallback(self, msg): ''' Receive planned trajectories and insert it into an array of waypoints ''' if (not self.odom_received): return # Variable to calculate the distance difference between 2 consecutive points last_pose = PoseGoal() last_pose.target_pose.pose.position.x = self.odometry.position.x last_pose.target_pose.pose.position.y = self.odometry.position.y last_pose.target_pose.pose.position.z = self.odometry.position.z last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w self.trajectory = [] last_motion_theta = 0 for t in msg.trajectory: for point in t.multi_dof_joint_trajectory.points: waypoint = PoseGoal() waypoint.target_pose.header.frame_id = "{}/world".format( self.robot_name) waypoint.target_pose.pose.position.x = point.transforms[ 0].translation.x waypoint.target_pose.pose.position.y = point.transforms[ 0].translation.y waypoint.target_pose.pose.position.z = point.transforms[ 0].translation.z # Orientate the robot always to the motion direction delta_x = point.transforms[ 0].translation.x - last_pose.target_pose.pose.position.x delta_y = point.transforms[ 0].translation.y - last_pose.target_pose.pose.position.y motion_theta = atan2(delta_y, delta_x) last_motion_theta = motion_theta # Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION # if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION): q = quaternion_from_euler(0, 0, motion_theta) waypoint.target_pose.pose.orientation.x = q[0] waypoint.target_pose.pose.orientation.y = q[1] waypoint.target_pose.pose.orientation.z = q[2] waypoint.target_pose.pose.orientation.w = q[3] # else: # waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x # waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y # waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z # waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w # Add a rotation inplace if next position has an angle difference bigger than 45 if abs(motion_theta - last_motion_theta) > 0.785: last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation self.trajectory.append(last_pose) last_pose = copy.copy( waypoint) # Save pose to calc the naxt delta last_motion_theta = motion_theta self.trajectory.append(waypoint) #Insert a last point to ensure that the robot end at the right position waypoint = PoseGoal() waypoint.target_pose.header.frame_id = "{}/world".format( self.robot_name) waypoint.target_pose.pose.position.x = point.transforms[ 0].translation.x waypoint.target_pose.pose.position.y = point.transforms[ 0].translation.y waypoint.target_pose.pose.position.z = point.transforms[ 0].translation.z waypoint.target_pose.pose.orientation.x = point.transforms[ 0].rotation.x waypoint.target_pose.pose.orientation.y = point.transforms[ 0].rotation.y waypoint.target_pose.pose.orientation.z = point.transforms[ 0].rotation.z waypoint.target_pose.pose.orientation.w = point.transforms[ 0].rotation.w self.trajectory.append(waypoint) self.trajectory_received = True def collisionCallback(self, feedback): ''' This callback runs on every feedback message received ''' validity_msg = GetStateValidityRequest( ) # Build message to verify collision validity_msg.group_name = PLANNING_GROUP if self.next_pose and (not self.collision): self.odometry_me.acquire() x = self.odometry.position.x y = self.odometry.position.y z = self.odometry.position.z # Distance between the robot and the next position dist = sqrt((self.next_pose.position.x - x)**2 + (self.next_pose.position.y - y)**2 + (self.next_pose.position.z - z)**2) # Pose to verify collision pose = Transform() pose.rotation.x = self.odometry.orientation.x pose.rotation.y = self.odometry.orientation.y pose.rotation.z = self.odometry.orientation.z pose.rotation.w = self.odometry.orientation.w self.odometry_me.release() #Verify possible collisions on diferent points between the robot and the goal point # rospy.logerr("\n\n\nCOLLISION CALLBACK: ") # rospy.logerr(dist) for d in arange(RESOLUTION, dist + 0.5, RESOLUTION): pose.translation.x = (self.next_pose.position.x - x) * (d / dist) + x pose.translation.y = (self.next_pose.position.y - y) * (d / dist) + y pose.translation.z = (self.next_pose.position.z - z) * (d / dist) + z self.current_pose.multi_dof_joint_state.transforms[ 0] = pose # Insert the correct odometry value validity_msg.robot_state = self.current_pose # Call service to verify collision collision_res = self.validity_srv.call(validity_msg) # print("\nCollision response:") # print(collision_res) # Check if robot is in collision if not collision_res.valid: # print(validity_msg) rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format( pose.translation.x, pose.translation.y, pose.translation.z)) rospy.logerr('Current pose [x:{} y:{} z:{}]'.format( x, y, z)) # print(collision_res) self.move_client.cancel_goal() self.collision = True return
class PickAndPlaceServer(object): def __init__(self): self.node_name = "PickAndPlaceServer" rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() # Get the object size self.object_height = 0.1 self.object_width = 0.05 self.object_depth = 0.05 self.pick_pose = rospy.get_param('~pickup_marker_pose') self.place_pose = rospy.get_param('~place_marker_pose') rospy.loginfo("%s: Waiting for pickup action server...", self.node_name) self.pickup_ac = SimpleActionClient('/pickup', PickupAction) connected = self.pickup_ac.wait_for_server(rospy.Duration(3000)) if not connected: rospy.logerr("%s: Could not connect to pickup action server", self.node_name) exit() rospy.loginfo("%s: Connected to pickup action server", self.node_name) rospy.loginfo("%s: Waiting for place action server...", self.node_name) self.place_ac = SimpleActionClient('/place', PlaceAction) if not self.place_ac.wait_for_server(rospy.Duration(3000)): rospy.logerr("%s: Could not connect to place action server", self.node_name) exit() rospy.loginfo("%s: Connected to place action server", self.node_name) self.scene = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact") else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start() def pick_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.grasp_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.pick_as.set_aborted(p_res) else: self.pick_as.set_succeeded(p_res) def place_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.place_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.place_as.set_aborted(p_res) else: self.place_as.set_succeeded(p_res) def wait_for_planning_scene_object(self, object_name='part'): rospy.loginfo( "Waiting for object '" + object_name + "'' to appear in planning scene...") gps_req = GetPlanningSceneRequest() gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: # This call takes a while when rgbd sensor is set gps_resp = self.scene_srv.call(gps_req) # check if 'part' is in the answer for collision_obj in gps_resp.scene.world.collision_objects: if collision_obj.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo("'" + object_name + "'' is in scene!") def grasp_object(self, object_pose): rospy.loginfo("Removing any previous 'part' object") self.scene.remove_attached_object("arm_tool_link") self.scene.remove_world_object("part") self.scene.remove_world_object("table") rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) # Removing is fast rospy.loginfo("Adding new 'part' object") rospy.loginfo("Object pose: %s", object_pose.pose) #Add object description in scene self.scene.add_box("part", object_pose, (self.object_depth, self.object_width, self.object_height)) rospy.loginfo("Second%s", object_pose.pose) table_pose = copy.deepcopy(object_pose) #define a virtual table below the object table_height = object_pose.pose.position.z - self.object_width/2 table_width = 1.8 table_depth = 0.5 table_pose.pose.position.z += -(2*self.object_width)/2 -table_height/2 table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table self.scene.add_box("table", table_pose, (table_depth, table_width, table_height)) # # We need to wait for the object part to appear self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("table") # compute grasps possible_grasps = self.sg.create_grasps_from_object_pose(object_pose) goal = createPickupGoal("arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.logdebug("Using torso result: " + str(result)) rospy.loginfo( "Pick result: " + str(moveit_error_dict[result.error_code.val])) # Remove table from world self.scene.remove_world_object("table") return result.error_code.val def place_object(self, object_pose): rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) possible_placings = self.sg.create_placings_from_object_pose( object_pose) # Try only with arm rospy.loginfo("Trying to place using only arm") goal = createPlaceGoal( object_pose, possible_placings, "arm", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo(str(moveit_error_dict[result.error_code.val])) if str(moveit_error_dict[result.error_code.val]) != "SUCCESS": rospy.loginfo( "Trying to place with arm and torso") # Try with arm and torso goal = createPlaceGoal( object_pose, possible_placings, "arm_torso", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.logerr(str(moveit_error_dict[result.error_code.val])) # print result rospy.loginfo( "Result: " + str(moveit_error_dict[result.error_code.val])) rospy.loginfo("Removing previous 'part' object") self.scene.remove_world_object("part") return result.error_code.val
class LocalSearch(): VISUAL_FIELD_SIZE = 40 MIN_HEAD_ANGLE = -140 MAX_HEAD_ANGLE = 140 _feedback = LocalSearchFeedback() _result = LocalSearchResult() def __init__(self): self._action_name = 'local_search' self.found_marker = False self.tracking_started = False #initialize head mover name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() rospy.loginfo('%s: Action client for PointHeadAction running' % self._action_name) #initialize tracker mark rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_tracker) rospy.loginfo('%s: subscribed to AlvarMarkers' % self._action_name) #initialize this client self._as = SimpleActionServer(self._action_name, LocalSearchAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def marker_tracker(self, marker): if self.tracking_started: self.found_marker = True rospy.loginfo('%s: marker found' % self._action_name) def run(self, goal): success = False self.tracking_started = True self.found_marker = False rospy.loginfo('%s: Executing search for AR marker' % self._action_name) # define a set of ranges to search search_ranges = [ # first search in front of the robot (0, self.VISUAL_FIELD_SIZE), (self.VISUAL_FIELD_SIZE, -self.VISUAL_FIELD_SIZE), # then search all directions (-self.VISUAL_FIELD_SIZE, self.MAX_HEAD_ANGLE), (self.MAX_HEAD_ANGLE, self.MIN_HEAD_ANGLE), (self.MIN_HEAD_ANGLE, 0) ] range_index = 0 #success = self.search_range(*(search_ranges[range_index])) while (not success) and range_index < len(search_ranges) - 1: if self._as.is_preempt_requested(): rospy.loginfo('%s: Premepted' % self._action_name) self._as.set_preempted() break range_index = range_index + 1 success = self.search_range(*(search_ranges[range_index])) if success: rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded() else: self._as.set_aborted() self.tracking_started = False def search_range(self, start_angle, end_angle): rospy.loginfo("{}: searching range {} {}".format(self._action_name, start_angle, end_angle)) angle_tick = self.VISUAL_FIELD_SIZE if (start_angle < end_angle) else -self.VISUAL_FIELD_SIZE for cur_angle in xrange(start_angle, end_angle, angle_tick): if self._as.is_preempt_requested(): return False head_goal = self.lookat_goal(cur_angle) rospy.loginfo('%s: Head move goal for %s: %s produced' % (self._action_name, str(cur_angle), str(head_goal))) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration.from_sec(5.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head could not move to specified location') break # pause at each tick rospy.sleep(0.3) if (self.found_marker): # found a marker! return True # no marker found return False def lookat_goal(self, angle): head_goal = PointHeadGoal() head_goal.target.header.frame_id = '/torso_lift_link' head_goal.max_velocity = 0.8 angle_in_radians = math.radians(angle) x = math.cos(angle_in_radians) * 5 y = math.sin(angle_in_radians) * 5 z = -0.5 head_goal.target.point = Point(x, y, z) return head_goal
class RobbieArmActionServer(): def __init__(self): # Initialize constants self.JOINTS_COUNT = 5 # Number of joints to manage self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME + 'server', anonymous=True) # Initialize publisher & subscriber for shoulder pan self.shoulder_pan_frame = 'arm_shoulder_tilt_link' self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0) self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64) rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan) rospy.wait_for_message('shoulder_pan_controller/state', JointState) # Initialize publisher & subscriber for arm tilt self.arm_tilt_frame = 'arm_pan_tilt_bracket' self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64) rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt) rospy.wait_for_message('arm_tilt_controller/state', JointState) # Initialize publisher & subscriber for elbow tilt self.elbow_tilt_frame = 'arm_bracket' #self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64) rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt) rospy.wait_for_message('elbow_tilt_controller/state', JointState) # Initialize publisher & subscriber for wrist pan self.wrist_pan_frame = 'wrist_pan_link' self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64) rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan) rospy.wait_for_message('wrist_pan_controller/state', JointState) # Initialize publisher & subscriber for wrist tilt self.wrist_tilt_frame = 'wrist_tilt_link' self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0) self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64) rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt) rospy.wait_for_message('wrist_tilt_controller/state', JointState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize joints action server self.result = RobbieArmResult() self.feedback = RobbieArmFeedback() self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback) # Reset arm position rospy.sleep(1) self.reset_arm_position() rospy.loginfo("%s: Ready to accept goals", NAME) def reset_arm_position(self): # reset arm to cobra position self.shoulder_pan_pub.publish(0.0) self.arm_tilt_pub.publish(1.572222) self.elbow_tilt_pub.publish(-1.572222) self.wrist_pan_pub.publish(0.0) self.wrist_tilt_pub.publish(0.0) rospy.sleep(12) def read_shoulder_pan(self, pan_data): self.shoulder_pan = pan_data self.has_latest_shoulder_pan = True def read_arm_tilt(self, tilt_data): self.arm_tilt = tilt_data self.has_latest_arm_tilt = True def read_elbow_tilt(self, tilt_data): self.elbow_tilt = tilt_data self.has_latest_elbow_tilt = True def read_wrist_pan(self, rotate_data): self.wrist_pan = rotate_data self.has_latest_wrist_pan = True def read_wrist_tilt(self, rotate_data): self.wrist_tilt = rotate_data self.has_latest_wrist_tilt = True def wait_for_latest_controller_states(self, timeout): self.has_latest_shoulder_pan = False self.has_latest_arm_tilt = False self.has_latest_elbow_tilt = False self.has_latest_wrist_pan = False self.has_latest_wrist_tilt = False r = rospy.Rate(100) start = rospy.Time.now() while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \ self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \ (rospy.Time.now() - start < timeout): r.sleep() def transform_target_point(self, point): rospy.loginfo("%s: Retrieving IK solutions", NAME) rospy.wait_for_service('smart_arm_ik_service', 10) ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK) resp = ik_service(point) if (resp and resp.success): return resp.solutions[0:4] else: raise Exception, "Unable to obtain IK solutions." def execute_callback(self, goal): r = rospy.Rate(100) self.result.success = True self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] rospy.loginfo("%s: Executing move arm", NAME) # Initialize target joints target_joints = list() for i in range(self.JOINTS_COUNT): target_joints.append(0.0) # Retrieve target joints from goal if (len(goal.target_joints) > 0): for i in range(min(len(goal.target_joints), len(target_joints))): target_joints[i] = goal.target_joints[i] else: try: # Convert target point to target joints (find an IK solution) target_joints = self.transform_target_point(goal.target_point) except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("%s: Aborted: IK Transform Failure", NAME) self.result.success = False self.server.set_aborted() return # Publish goal to controllers self.shoulder_pan_pub.publish(target_joints[0]) self.arm_tilt_pub.publish(target_joints[1]) self.elbow_tilt_pub.publish(target_joints[2]) self.wrist_pan_pub.publish(target_joints[3]) self.wrist_tilt_pub.publish(target_joints[4]) # Initialize loop variables start_time = rospy.Time.now() while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD): # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() break # Publish current arm position as feedback self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > self.TIMEOUT_THRESHOLD): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() break r.sleep() if (self.result.success): rospy.loginfo("%s: Goal Completed", NAME) self.wait_for_latest_controller_states(rospy.Duration(2.0)) self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \ self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value] self.server.set_succeeded(self.result)
class MockNavigation(): def __init__(self, move_base_topic): self.robot_pose_ = PoseStamped() self.listener = tf.TransformListener() self.navigation_succedes = True self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() self.move_base_as_ = SimpleActionServer( move_base_topic, MoveBaseAction, execute_cb = self.move_base_cb, auto_start = False) self.move_base_as_.start() def __del__(self): self.move_base_as_.__del__() def move_base_cb(self, goal): rospy.loginfo('move_base_cb') self.target_position = goal.target_pose.pose.position self.target_orientation = goal.target_pose.pose.orientation self.moves_base = True while not self.reply: rospy.sleep(0.2) (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.robot_pose_.pose.position.x = trans[0] self.robot_pose_.pose.position.y = trans[1] feedback = MoveBaseFeedback() feedback.base_position.pose.position.x = \ self.robot_pose_.pose.position.x feedback.base_position.pose.position.y = \ self.robot_pose_.pose.position.y self.move_base_as_.publish_feedback(feedback) if self.move_base_as_.is_preempt_requested(): self.preempted += 1 rospy.loginfo("Preempted!") self.moves_base = False self.move_base_as_.set_preempted() return None if self.move_base_as_.is_new_goal_available(): self.preempted += 1 self.move_base_cb(self.move_base_as_.accept_new_goal()) return None else: result = MoveBaseResult() self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() if self.navigation_succedes: self.move_base_as_.set_succeeded(result) else: self.move_base_as_.set_aborted(result)
class ShakePitchObjectActionServer: def __init__(self): self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording) self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording) self.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity) self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64) self.action_server = SimpleActionServer(ACTION_NAME, ShakePitchObjectAction, execute_cb=self.shake_pitch_object, auto_start=False) def initialize(self): rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME) rospy.wait_for_service('/wubble_grasp_status') rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/start_audio_recording') rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.wait_for_service('audio_dump/stop_audio_recording') rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME) rospy.loginfo('%s: waiting for wrist_pitch_controller service' % ACTION_NAME) rospy.wait_for_service('/wrist_pitch_controller/set_velocity') rospy.loginfo('%s: connected to wrist_pitch_controller service' % ACTION_NAME) self.action_server.start() def shake_pitch_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() wrist_pitch_state = '/wrist_pitch_controller/state' desired_velocity = 6.0 distance = 1.0 threshold = 0.1 timeout = 2.0 try: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position start_pos = current_pos # set wrist to initial position self.wrist_pitch_velocity_srv(3.0) self.wrist_pitch_command_pub.publish(distance) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos < distance-threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) self.wrist_pitch_velocity_srv(desired_velocity) # start recording sound and shaking self.start_audio_recording_srv(InfomaxAction.SHAKE_PITCH, goal.category_id) rospy.sleep(0.5) for i in range(2): self.wrist_pitch_command_pub.publish(-distance) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos > -distance+threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) self.wrist_pitch_command_pub.publish(distance) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos < distance-threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) rospy.sleep(0.5) # check if are still holding an object after shaking sound_msg = None grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) # reset wrist to starting position self.wrist_pitch_velocity_srv(3.0) self.wrist_pitch_command_pub.publish(start_pos) end_time = rospy.Time.now() + rospy.Duration(timeout) while current_pos < distance-threshold and rospy.Time.now() < end_time: msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout) current_pos = msg.position rospy.sleep(10e-3) rospy.sleep(0.5) if sound_msg: self.action_server.set_succeeded(ShakePitchObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return except: rospy.logerr('%s: attempted pitch failed - %s' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
class ErraticBaseActionServer(): def __init__(self): self.base_frame = '/base_footprint' self.move_client = SimpleActionClient('move_base', MoveBaseAction) self.move_client.wait_for_server() self.tf = tf.TransformListener() self.result = ErraticBaseResult() self.feedback = ErraticBaseFeedback() self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False) self.server.start() rospy.loginfo("%s: Ready to accept goals", NAME) def transform_target_point(self, point): self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0)) return self.tf.transformPoint(self.base_frame, point) def move_to(self, target_pose): goal = MoveBaseGoal() goal.target_pose = target_pose goal.target_pose.header.stamp = rospy.Time.now() self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb) while not self.move_client.wait_for_result(rospy.Duration(0.01)): # check for preemption if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.move_client.cancel_goal() return GoalStatus.PREEMPTED return self.move_client.get_state() def move_base_feedback_cb(self, fb): self.feedback.base_position = fb.base_position if self.server.is_active(): self.server.publish_feedback(self.feedback) def get_vicinity_target(self, target_pose, vicinity_range): vicinity_pose = PoseStamped() # transform target to base_frame reference target_point = PointStamped() target_point.header.frame_id = target_pose.header.frame_id target_point.point = target_pose.pose.position self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0)) target = self.tf.transformPoint(self.base_frame, target_point) rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z) # find distance to point dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2)) if (dist < vicinity_range): # if already within range, then no need to move vicinity_pose.pose.position.x = 0.0 vicinity_pose.pose.position.y = 0.0 else: # normalize vector pointing from source to target target.point.x /= dist target.point.y /= dist # scale normal vector to within vicinity_range distance from target target.point.x *= (dist - vicinity_range) target.point.y *= (dist - vicinity_range) # add scaled vector to source vicinity_pose.pose.position.x = target.point.x + 0.0 vicinity_pose.pose.position.y = target.point.y + 0.0 # set orientation ori = Quaternion() yaw = math.atan2(target.point.y, target.point.x) (ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw) vicinity_pose.pose.orientation = ori # prep header vicinity_pose.header = target_pose.header vicinity_pose.header.frame_id = self.base_frame rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z) return vicinity_pose def execute_callback(self, goal): rospy.loginfo("%s: Executing move base", NAME) move_base_result = None if goal.vicinity_range == 0.0: # go to exactly move_base_result = self.move_to(goal.target_pose) else: # go near (within vicinity_range meters) vicinity_target_pose = self.get_vicinity_target(goal.target_pose, goal.vicinity_range) move_base_result = self.move_to(vicinity_target_pose) # check results if (move_base_result == GoalStatus.SUCCEEDED): rospy.loginfo("%s: Succeeded", NAME) self.result.base_position = self.feedback.base_position self.server.set_succeeded(self.result) elif (move_base_result == GoalStatus.PREEMPTED): rospy.loginfo("%s: Preempted", NAME) self.server.set_preempted() else: rospy.loginfo("%s: Aborted", NAME) self.server.set_aborted()
class AggressiveGainBuffNode(object): def __init__(self, name="aggressive_gain_buff_action"): self.action_name = name self._as = SimpleActionServer(self.action_name, AggressiveGainBuffAction, execute_cb=self.ExecuteCB, auto_start=False) self.gain_buff_state = GainBuffState.ROUTE self.chassis_state_ = 0 self.sub_odom = rospy.Subscriber("aggressive_buff_info", AggressiveGainBuffInfo, callback=self.OdomCB) self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode", ChassisMode) self.chassis_arrive_state = ChassisArriveState() self.chassis_mode_ = 0 self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction) rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...') ret = self._ac_navto.wait_for_server() rospy.loginfo('GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr('error: NavTo server not started!') self.nav_to_error_code = -1 self.search_start_time = rospy.Time(0) self._ac_look_n_move = SimpleActionClient("look_n_move_node_action", LookAndMoveAction) rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...') ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3)) rospy.loginfo('GAIN_BUFF: Look and Move sever connected!') if ret else rospy.logerr('error: Look and Move server not started!') self.Look_n_move_error_code = -1 self.thread_ = Thread(target=self.Loop) self.thread_.start() self._as.start() def ExecuteCB(self, goal): print 'GAIN_BUFF:Aggressive gain buff goal recieved!' route = goal.route_index CHASSIS_MODE = ChassisModeForm() self.nav_to_error_code = -1 while not rospy.is_shutdown(): if self._as.is_preempt_requested(): print 'GAIN_BUFF:PREEMPT REQ' self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self._ac_navto.cancel_all_goals() self._ac_look_n_move.cancel_all_goals() self._as.set_preempted() return if self.gain_buff_state == GainBuffState.ROUTE: if route == 1 or route == 2: print 'GAIN_BUFF:Route %i received' % route if route == 1: self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_ONE) if route == 2: self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_TWO) print 'GAIN_BUFF:Wait for chassis response...' chassis_wait_start_time = rospy.get_time() print 'GAIN_BUFF:chassis state is %i' % (self.chassis_state_) while self.chassis_state_ == 0 and (rospy.get_time() - chassis_wait_start_time) < CHASSIS_MODE_WAIT_TIME: continue if self.chassis_state_ == 1: print 'GAIN_BUFF:Chassis has responded!' else: print 'GAIN_BUFF:Chassis does not respond for the new mode! Return.' self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self._as.set_aborted() return chassis_run_start_time = rospy.get_time() while not (self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED or (rospy.get_time()-chassis_run_start_time > CHASSIS_RUN_WAIT_TIME) ): if self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED: # self._as.set_succeeded() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self.gain_buff_state = GainBuffState.SEARCH self.search_start_time = rospy.get_time() print 'GAIN_BUFF:Chassis Arrived Buff Area!' break else: print 'GAIN_BUFF:Chassis Aggressive gain buff FAILED, Time out!' self._as.set_aborted() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) return elif route == 3: print 'GAIN_BUFF:Route 3 received' self.NavToBuff() if self.nav_to_error_code == 0: self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) # self._as.set_succeeded() self.gain_buff_state = GainBuffState.SEARCH self.search_start_time = rospy.get_time() print 'GAIN_BUFF:Aggressive Gain Buff Route 3 SUCCEED!' else: self._as.set_aborted() print 'No valied route' return elif self.gain_buff_state == GainBuffState.SEARCH: if (rospy.get_time() - self.search_start_time) < SEARCH_BUFF_WAIT_TIME: self.SearchBuff() else: print 'SEARCH BUFF: Chassis Aggressive search buff Time Out!' self._as.set_aborted() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) self.gain_buff_state = GainBuffState.IDEL return else: print 'Unvaild aggressive gain buff state!' self._as.set_aborted() self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL) return def Loop(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): if self._as.is_preempt_requested(): self._ac_look_n_move.cancel_all_goals() self._ac_navto.cancel_all_goals() try: rate.sleep() except: rospy.loginfo('GAIN_BUFF: exiting') # Set chassis mode to use odom navigation def SetChassisMode(self, chassis_mode): if self.chassis_mode_ == chassis_mode: return rospy.wait_for_service("set_chassis_mode", timeout=5) print 'Set chassis mode service connected!' chassis_mode_msg = ChassisMode() call_status = self.chassis_mode_client.call(chassis_mode) chassis_mode_rec_ = ChassisModeResponse() chassis_mode_rec_ = chassis_mode_msg._response_class if(call_status and chassis_mode_rec_): self.chassis_mode_ = chassis_mode print 'Set Chassis Mode to %i' % chassis_mode else: print 'Set gimbal mode failed!' # return odom arrive result def OdomCB(self, data): self.chassis_state_ = data.state # Call NavTo Action def NavToBuff(self): print 'NavTo action is actived!' g = NavToActionGoal() for path in BuffPath: q = tf.transformations.quaternion_from_euler(0,0,path['yaw']) g.goal.navgoal.pose.position.x = path['x'] g.goal.navgoal.pose.position.y = path['y'] g.goal.navgoal.pose.orientation.z = q[2] g.goal.navgoal.pose.orientation.w = q[3] self._ac_navto.send_goal(g.goal, done_cb=self.done_cb, feedback_cb=None) print 'Waiting for %i point ...' % (path['id']) self._ac_navto.wait_for_result(timeout=rospy.Duration(10)) if self.nav_to_error_code != 0: self._ac_navto.cancel_all_goals() self._as.set_aborted() print 'AGGRESSIVA_GAIN_BUFF: NavTo Failed!' return print 'The result of %i point in BuffPath is arrived!' % path['id'] def SearchBuff(self): print 'Look and Move action is actived!' for goal in SearchBuffGoal: q = tf.transformations.quaternion_from_euler(0.,0.,goal['yaw']) g = LookAndMoveActionGoal() g.goal.relative_pose.header.frame_id = "map" g.goal.relative_pose.pose.position.x = goal['x'] g.goal.relative_pose.pose.position.y = goal['y'] g.goal.relative_pose.pose.orientation.z = q[2] g.goal.relative_pose.pose.orientation.w = q[3] self._ac_look_n_move.send_goal(g.goal) self._ac_look_n_move.wait_for_result(timeout=rospy.Duration(10)) if self.Look_n_move_error_code > 0: self._ac_look_n_move.cancel_all_goals() self._as.set_aborted() print 'AGGRESSIVA_GAIN_BUFF: Look and Move Failed!' return # Nav_to done result def done_cb(self,terminal_state,result): self.nav_to_error_code = result.error_code
class ObjectDetectionActionServer(object): _detector_handler = None # type: SingleImageDetectionHandler _cloud_topic = None # type: str _filtered_cloud_pub = None # type: rospy.Publisher _tf_listener = None # type: tf.TransformListener _target_frame = None # type: str _cv_bridge = None # type: CvBridge def __init__(self, action_name, timeout_s=10., **kwargs): rospy.loginfo('broadcasting action server: ' + action_name) self._action_server = SimpleActionServer(action_name, DetectObjectsAction, execute_cb=self._execute_cb, auto_start=False) self._timeout_s = timeout_s self._initialize(**kwargs) self._action_server.start() def _initialize(self, **kwargs): detection_class = kwargs.get('detection_class', None) if not issubclass(detection_class, ImageDetectorBase): raise ValueError( '"detection_class" is not of ImageDetectorBase type') class_annotation_file = kwargs.get('class_annotation_file', None) if not class_annotation_file or not os.path.exists( class_annotation_file): raise ValueError('invalid value for "class_annotation_file": ' + class_annotation_file) kwargs_file = kwargs.get('kwargs_file', None) if not kwargs_file or not os.path.exists(kwargs_file): raise ValueError('invalid value for "kwargs_file": ' + kwargs_file) # image detection self._detector_handler = SingleImageDetectionHandler( detection_class, class_annotation_file, kwargs_file, '/mas_perception/detection_result') self._cloud_topic = kwargs.get('cloud_topic', None) if not self._cloud_topic: raise ValueError('no cloud topic specified') self._filtered_cloud_pub = rospy.Publisher('filtered_cloud', PointCloud2, queue_size=1) self._tf_listener = tf.TransformListener() self._target_frame = kwargs.get('target_frame', '/base_link') rospy.loginfo('will transform all poses to frame: ' + self._target_frame) def _execute_cb(self, _): # subscribe and wait for cloud message rospy.loginfo('Object detection action request received') rospy.loginfo('Waiting for cloud message...') try: cloud_msg = rospy.wait_for_message(self._cloud_topic, PointCloud2, timeout=self._timeout_s) except rospy.ROSInterruptException: # shutdown event interrupts waiting for point cloud raise except rospy.ROSException: rospy.logwarn( 'timeout waiting for cloud topic "{0}" after "{1}" seconds'. format(self._cloud_topic, self._timeout_s)) self._action_server.set_aborted() return rospy.loginfo('Cloud message received; detecting objects...') img_msg = cloud_msg_to_image_msg(cloud_msg) original_img_msg = copy.deepcopy(img_msg) try: bounding_boxes, classes, confidences = self._detector_handler.process_image_msg( img_msg) except RuntimeError as e: self._action_server.set_aborted(text=e.message) return rospy.loginfo('transforming cloud to frame: ' + self._target_frame) try: transformed_cloud_msg = transform_cloud_with_listener( cloud_msg, self._target_frame, self._tf_listener) except RuntimeError as e: self._action_server.set_aborted(text=e.message) return if self._filtered_cloud_pub.get_num_connections() > 0: self._filtered_cloud_pub.publish(filtered_cloud) rospy.loginfo('creating action result and setting success') result = ObjectDetectionActionServer._get_action_result( transformed_cloud_msg, bounding_boxes, classes, confidences) result.image = original_img_msg self._action_server.set_succeeded(result) @staticmethod def _get_action_result(cloud_msg, bounding_boxes, classes, confidences): """ :type cloud_msg: PointCloud2 :type bounding_boxes: list :type classes: list :type confidences: list :rtype: DetectObjectsResult """ result = DetectObjectsResult() for index, box in enumerate(bounding_boxes): detected_obj = get_obj_msg_from_detection( cloud_msg, box, classes[index], confidences[index], cloud_msg.header.frame_id) box_msg = detected_obj.bounding_box rospy.loginfo( "Adding object '%s', position (%.3f, %.3f, %.3f), dimensions (%.3f, %.3f, %.3f)" % (classes[index], box_msg.center.x, box_msg.center.y, box_msg.center.z, box_msg.dimensions.x, box_msg.dimensions.y, box_msg.dimensions.z)) result.objects.objects.append(detected_obj) return result
class FakeTurtle: def __init__(self): # define the server, aut-start is set to false so we control when to # start it self.server = SimpleActionServer('move_turtle_action', MoveTurtleAction, execute_cb=self.action_cb, auto_start=False) # deine feedback msg object to be used for publishing feedback self.feedback_msg = MoveTurtleFeedback() # deine result msg object to be used for publishing result self.result_msg = MoveTurtleResult() self.server.start() print("action server started ..") self.rate = rospy.Rate(2) def turtle_is_up_side_down(self): # this turtle never goes up side down # it can always move! # (this is a dummy function, in real application you might be checking # your robot status, for example the battery level, # or motor temperature, etc..) return False def action_cb(self, goal): print("moving turtle to pose: ", goal) # Write here the logic to control turtle # this "for" loop is just a demostration, in real application, you # would be controlling the robot, publish velocity commands, read # current pose and do some computation.. for _ in range(20): print('executing goal') # during execution of the goal, publish feedback msg to the client self.server.publish_feedback(self.feedback_msg) # always check if the client has sent a cancel msg. This way the # task becomes preemtable if self.server.is_preempt_requested(): # update goal status accordingly self.server.set_preempted() print('goal preempted') # exit from the action_cb method return None # check if we need to abort for some reason. Here we abort if # the node has been killed (ctrl+C) or if the turtle is laying on # it's back and cannot move anyomre ;) if rospy.is_shutdown() or self.turtle_is_up_side_down(): # update goal status accordingly self.server.set_aborted() print('Turtle cannot move sir! goal aborted') return None self.rate.sleep() # if execution finished, set goal status to SUCCEED and publish the # result msg print('done') self.server.set_succeeded(result=self.result_msg, text="Mission completed")
class RobbieHeadActionServer(): def __init__(self): # Initialize constants self.JOINTS_COUNT = 2 # Number of joints to manage self.ERROR_THRESHOLD = 0.02 # Report success if error reaches below threshold (0.015 also works) self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold # Initialize new node rospy.init_node(NAME, anonymous=True) # Initialize publisher & subscriber for pan self.head_pan_frame = 'head_pan_link' self.head_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.head_pan_pub = rospy.Publisher('head_pan_controller/command', Float64) rospy.Subscriber('head_pan_controller/state_pr2_msgs', JointControllerState, self.read_current_pan) rospy.wait_for_message('head_pan_controller/state_pr2_msgs', JointControllerState) # Initialize publisher & subscriber for tilt self.head_tilt_frame = 'head_tilt_link' self.head_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0) self.head_tilt_pub = rospy.Publisher('head_tilt_controller/command', Float64) rospy.Subscriber('head_tilt_controller/state_pr2_msgs', JointControllerState, self.read_current_tilt) rospy.wait_for_message('head_tilt_controller/state_pr2_msgs', JointControllerState) # Initialize tf listener self.tf = tf.TransformListener() # Initialize point action server self.result = RobbieHeadResult() self.feedback = RobbieHeadFeedback() self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value] self.server = SimpleActionServer(NAME, RobbieHeadAction, self.execute_callback, auto_start=False) # Reset head position rospy.sleep(1) self.reset_head_position() rospy.loginfo("%s: Ready to accept goals", NAME) def read_current_pan(self, pan_data): self.head_pan = pan_data self.has_latest_pan = True def read_current_tilt(self, tilt_data): self.head_tilt = tilt_data self.has_latest_tilt = True def reset_head_position(self): self.head_pan_pub.publish(0.0) self.head_tilt_pub.publish(0.0) rospy.sleep(5) def wait_for_latest_controller_states(self, timeout): self.has_latest_pan = False self.has_latest_tilt = False r = rospy.Rate(100) start = rospy.Time.now() while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout): r.sleep() def transform_target_point(self, point): pan_target_frame = self.head_pan_frame tilt_target_frame = self.head_tilt_frame # Wait for tf info (time-out in 5 seconds) self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0)) self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0)) # Transform target point to pan reference frame & retrieve the pan angle pan_target = self.tf.transformPoint(pan_target_frame, point) pan_angle = math.atan2(pan_target.point.y, pan_target.point.x) #rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \ # NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle) # Transform target point to tilt reference frame & retrieve the tilt angle tilt_target = self.tf.transformPoint(tilt_target_frame, point) tilt_angle = math.atan2(tilt_target.point.z, math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2))) #rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \ # NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle) return [pan_angle, tilt_angle] def execute_callback(self, goal): r = rospy.Rate(100) self.result.success = True self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value] rospy.loginfo("%s: Executing move head", NAME) # Initialize target joints target_joints = list() for i in range(self.JOINTS_COUNT): target_joints.append(0.0) # Retrieve target joints from goal if (len(goal.target_joints) > 0): for i in range(min(len(goal.target_joints), len(target_joints))): target_joints[i] = goal.target_joints[i] else: try: # Try to convert target point to pan & tilt angles target_joints = self.transform_target_point(goal.target_point) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("%s: Aborted: Transform Failure", NAME) self.result.success = False self.server.set_aborted() return # Publish goal command to controllers self.head_pan_pub.publish(target_joints[0]) self.head_tilt_pub.publish(target_joints[1]) # Initialize loop variables start_time = rospy.Time.now() while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \ math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD): # Cancel exe if another goal was received (i.e. preempt requested) if self.server.is_preempt_requested(): rospy.loginfo("%s: Aborted: Action Preempted", NAME) self.result.success = False self.server.set_preempted() break # Publish current head position as feedback self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value] self.server.publish_feedback(self.feedback) # Abort if timeout current_time = rospy.Time.now() if (current_time - start_time > self.TIMEOUT_THRESHOLD): rospy.loginfo("%s: Aborted: Action Timeout", NAME) self.result.success = False self.server.set_aborted() break r.sleep() if (self.result.success): rospy.loginfo("%s: Goal Completed", NAME) self.wait_for_latest_controller_states(rospy.Duration(2.0)) self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value] self.server.set_succeeded(self.result)
class V_search(object): def __init__(self, height, res): self.resolution = res # Resolution of the motion self.motion_height = height # Height of the motion to the ground # Create search server self.search_server = SimpleActionServer('search_server', ExecuteSearchAction, self.searchCallback, False) self.server_feedback = ExecuteSearchFeedback() self.server_result = ExecuteSearchResult() # Get client from trajectory server self.trajectory_client = SimpleActionClient( "approach_server", ExecuteDroneApproachAction) self.trajectory_client.wait_for_server() self.next_point = ExecuteDroneApproachGoal( ) # Message to define next position to look for victims ## variables self.sonar_me = Condition() self.odometry_me = Condition() self.current_height = None self.odometry = None # Subscribe to sonar_height rospy.Subscriber("sonar_height", Range, self.sonar_callback, queue_size=10) # Subscribe to ground_truth to monitor the current pose of the robot rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback) # Start trajectory server self.search_server.start() def trajectory_feed(self, msg): ''' Verifies preemption requisitions ''' if self.search_server.is_preempt_requested(): self.trajectory_client.cancel_goal() def searchCallback(self, search_area): ''' Execute a search for vitims into the defined area ''' x = search_area.x # x size of the area to explore y = search_area.y # y size of the area to explore start = search_area.origin # Point to start the search self.next_point.goal.position.x = start.x # Desired x position self.next_point.goal.position.y = start.y # Desired y position theta = 0 # Convert desired angle q = quaternion_from_euler(0, 0, theta, 'ryxz') self.next_point.goal.orientation.x = q[0] self.next_point.goal.orientation.y = q[1] self.next_point.goal.orientation.z = q[2] self.next_point.goal.orientation.w = q[3] x_positions = ceil(x / self.resolution) y_positions = ceil(y / self.resolution) x_count = 0 # Counter of steps (in meters traveled) direction = 1 # Direction of the motion (right, left or up) trials = 0 # v_search trials while not rospy.is_shutdown(): self.odometry_me.acquire() self.server_result.last_pose = self.odometry self.server_feedback.current_pose = self.odometry self.odometry_me.release() if self.search_server.is_preempt_requested(): self.search_server.set_preempted(self.server_result) return self.search_server.publish_feedback(self.server_feedback) self.sonar_me.acquire() # print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground h_error = self.motion_height - self.current_height self.sonar_me.release() self.odometry_me.acquire() self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position self.odometry_me.release() self.trajectory_client.send_goal(self.next_point, feedback_cb=self.trajectory_feed) self.trajectory_client.wait_for_result() # Wait for the result result = self.trajectory_client.get_state( ) # Get the state of the action # print(result) if result == GoalStatus.SUCCEEDED: # Verifies if all the area have been searched if (self.next_point.goal.position.x == (start.x + x)) and ( (self.next_point.goal.position.y == (start.y + y))): self.odometry_me.acquire() self.server_result.last_pose = self.odometry self.odometry_me.release() self.search_server.set_succeeded(self.server_result) return last_direction = direction direction = self.square_function( x_count, 2 * x) # Get the direction of the next step if last_direction != direction: # drone moves on y axis theta = pi / 2 self.next_point.goal.position.y += y / y_positions elif direction == 1: # drone moves to the right theta = 0 self.next_point.goal.position.x += x / x_positions x_count += x / x_positions elif direction == -1: # drone moves to the left theta = pi self.next_point.goal.position.x -= x / x_positions x_count += x / x_positions # Convert desired angle q = quaternion_from_euler(0, 0, theta, 'ryxz') self.next_point.goal.orientation.x = q[0] self.next_point.goal.orientation.y = q[1] self.next_point.goal.orientation.z = q[2] self.next_point.goal.orientation.w = q[3] elif result == GoalStatus.ABORTED: trials += 1 if trials == 2: self.search_server.set_aborted(self.server_result) return def square_function(self, x, max_x): ''' Function to simulate a square wave function ''' if round(sin(2 * pi * x / max_x), 2) > 0: return 1 elif round(sin(2 * pi * x / max_x), 2) < 0: return -1 else: if round(cos(2 * pi * x / max_x), 2) > 0: return 1 else: return -1 def sonar_callback(self, msg): ''' Function to update drone height ''' self.sonar_me.acquire() self.current_height = msg.range self.sonar_me.release() def poseCallback(self, odometry): ''' Monitor the current position of the robot ''' self.odometry_me.acquire() self.odometry = odometry.pose.pose self.odometry_me.release()