import geometry_msgs.msg if __name__ == '__main__': # sleep_time = float(sys.argv[1]) sleep_time = 1.0 print "sleep_time", sleep_time time.sleep(sleep_time) node = rospy.init_node('action_test_1') # =============================================== ''' 1.Test unit for platform moving ''' # =============================================== client = SimpleActionClient( '/position_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() print 'OK' goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ 'move_1_joint', 'move_2_joint', 'move_3_joint' ] point1 = JointTrajectoryPoint() point1.time_from_start = rospy.Duration.from_sec(0.3) print point1.time_from_start point1.positions = [0, 0, 0] # point2 = JointTrajectoryPoint() # point2.time_from_start = rospy.Duration.from_sec(0.6)
def __init__(self, name): ActionServer.__init__(self, name, DockAction, self.__goal_callback, self.__cancel_callback, False) self.__active = False self.__docked = False self.__mutex = threading.Lock() self.__dock_speed = rospy.get_param('~dock/dock_speed', 0.05) self.__dock_distance = rospy.get_param('~dock/dock_distance', 1.0) self.__map_frame = rospy.get_param('~map_frame', 'map') self.__odom_frame = rospy.get_param('~odom_frame', 'odom') self.__base_frame = rospy.get_param('~base_frame', 'base_footprint') self.__dock_ready_pose = Pose() self.__dock_ready_pose.position.x = rospy.get_param('~dock/pose_x') self.__dock_ready_pose.position.y = rospy.get_param('~dock/pose_y') self.__dock_ready_pose.position.z = rospy.get_param('~dock/pose_z') self.__dock_ready_pose.orientation.x = rospy.get_param( '~dock/orientation_x') self.__dock_ready_pose.orientation.y = rospy.get_param( '~dock/orientation_y') self.__dock_ready_pose.orientation.z = rospy.get_param( '~dock/orientation_z') self.__dock_ready_pose.orientation.w = rospy.get_param( '~dock/orientation_w') self.__dock_ready_pose_2 = PoseStamped() rospy.loginfo('param: dock_spped, %s, dock_distance %s' % (self.__dock_speed, self.__dock_distance)) rospy.loginfo('param: map_frame %s, odom_frame %s, base_frame %s' % (self.__map_frame, self.__odom_frame, self.__base_frame)) rospy.loginfo('dock_pose:') rospy.loginfo(self.__dock_ready_pose) self.__movebase_client = SimpleActionClient('move_base', MoveBaseAction) rospy.loginfo('wait for movebase server...') self.__movebase_client.wait_for_server() rospy.loginfo('movebase server connected') self.__approach_path = Path() self.__path_tracker = PathTracker() self.__approach_path_pub = rospy.Publisher('dock_approach_path', Path, queue_size=10) self.__cmd_pub = rospy.Publisher( 'yocs_cmd_vel_mux/input/navigation_cmd', Twist, queue_size=10) rospy.Subscriber('dock_pose', PoseStamped, self.__dock_pose_callback) self.__tf_listener = tf.TransformListener() self.__docked = False # self.__saved_goal = MoveBaseGoal() self.__no_goal = True self.__current_goal_handle = ServerGoalHandle() self.__exec_condition = threading.Condition() self.__exec_thread = threading.Thread(None, self.__exec_loop) self.__exec_thread.start() rospy.loginfo('Creating ActionServer [%s]\n', name)
def __init__(self): """ This constructor initialises the different necessary connections to the topics and services and resets the world to start in a good position. """ rospy.init_node("smart_grasper") self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.__joint_state_cb, queue_size=1) rospy.wait_for_service("/gazebo/get_model_state", 10.0) rospy.wait_for_service("/gazebo/reset_world", 10.0) self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty) self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) rospy.wait_for_service("/gazebo/pause_physics") self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty) rospy.wait_for_service("/gazebo/unpause_physics") self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) rospy.wait_for_service("/controller_manager/switch_controller") self.__switch_ctrl = rospy.ServiceProxy( "/controller_manager/switch_controller", SwitchController) rospy.wait_for_service("/gazebo/set_model_configuration") self.__set_model = rospy.ServiceProxy( "/gazebo/set_model_configuration", SetModelConfiguration) rospy.wait_for_service('/get_planning_scene', 10.0) self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True) self.arm_commander = MoveGroupCommander("arm") self.hand_commander = MoveGroupCommander("hand") self.__hand_traj_client = SimpleActionClient( "/hand_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.__arm_traj_client = SimpleActionClient( "/arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) if self.__hand_traj_client.wait_for_server( timeout=rospy.Duration(4.0)) is False: rospy.logfatal( "Failed to connect to /hand_controller/follow_joint_trajectory in 4sec." ) raise Exception( "Failed to connect to /hand_controller/follow_joint_trajectory in 4sec." ) if self.__arm_traj_client.wait_for_server( timeout=rospy.Duration(4.0)) is False: rospy.logfatal( "Failed to connect to /arm_controller/follow_joint_trajectory in 4sec." ) raise Exception( "Failed to connect to /arm_controller/follow_joint_trajectory in 4sec." ) self.reset_world()
if __name__ == '__main__': argv = rospy.myargv() if len(argv) != 3: print("Usage:") print( argv[0] + " <command executer action server> <yaml file with list of commands>" ) print("") print("Example:") print(argv[0] + " /main_computer/command list_of_commands.yaml") exit(0) print("Loading commands from " + argv[2]) msgs = yaml_to_msgs(argv[2]) print("Loaded " + str(len(msgs)) + " commands.") print("Connecting to action server: " + argv[1]) rospy.init_node('commands_from_yaml') ac = SimpleActionClient(argv[1], ExecuteCommandAction) d = rospy.Duration(5.0) while not rospy.is_shutdown() and not ac.wait_for_server(d): print("Waiting for action server " + argv[1] + " ...") for msg in msgs: print("\nSending command:\n" + str(msg)[:200] + "...") ac.send_goal(msg) time.sleep(0.1) print("All commands sent.")
def __init__(self, x, y, z): self._x = x self._y = y self._z = z # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface() #未知 self._robot = RobotCommander() #未知 self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name, self._x, self._y, self._z) rospy.sleep(0.5) # Define target place pose:定义目标位置姿势 self._pose_place = Pose() self._pose_place.position.x = 0 #self._pose_coke_can.position.x self._pose_place.position.y = -0.85 #self._pose_coke_can.position.y - 0.20 self._pose_place.position.z = 0.7 #self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('up') self._arm.go(wait=True) print("up pose") print("第一部分恢复位置初始") # Create grasp generator 'generate' action client: # #创建抓取生成器“生成”动作客户端: print("开始Action通信") self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return print("结束Action通信") # Create move group 'pickup' action client: # 创建移动组“抓取”操作客户端: print("开始pickup通信") self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return print("结束pickup通信") # Create move group 'place' action client: # 创建移动组“放置”动作客户端: print("开始place通信") self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return print("结束place通信") # Pick Coke can object:抓取快 while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(0.5) print("抓取物体") rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(0.5) rospy.loginfo('Place successfully')
def __init__(self, ns, action_spec): self._client = SimpleActionClient(ns, action_spec) self._client.wait_for_server()
header.stamp = rospy.Time.now() grasp_PA.header = header for graspmsg in grasps: p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation) grasp_PA.poses.append(p) grasp_publisher.publish(grasp_PA) rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses') rospy.sleep(2) if __name__ == '__main__': name = 'grasp_object_server' rospy.init_node(name, anonymous=False) rospy.loginfo("Connecting to grasp generator AS") grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) gripper_group = rospy.get_param('~gripper', 'gripper') grasp_publisher = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) object_pose = geometry_msgs.msg.PoseStamped() object_pose.pose.position.x = 0.0 object_pose.pose.position.y = 0.0 object_pose.pose.position.z = 0.0 object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 grasp_list = generate_grasps(object_pose, 0.02) rospy.loginfo('Generated ' + str(len(grasp_list)) + ' grasps.')
def __init__(self): super(NavigateToGoalState, self).__init__(outcomes=['ok', 'err'], input_keys=['target_pose']) self.client = SimpleActionClient('move_base', MoveBaseAction)
def __init__(self): self.gripper_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction) self.gripper_client.wait_for_server()
#! /usr/bin/env python import sys import rospy from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from diagnostic_msgs.msg import KeyValue from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal if __name__ == "__main__": rospy.init_node("perceive_location_client_tester") client = SimpleActionClient("perceive_location_server", GenericExecuteAction) client.wait_for_server() goal = GenericExecuteGoal() if len(sys.argv) > 1: location = str(sys.argv[1]).upper() goal.parameters.append(KeyValue(key="location", value=location)) rospy.loginfo("Sending following goal to perceive location server") rospy.loginfo(goal) client.send_goal(goal) timeout = 15.0 finished_within_time = client.wait_for_result( rospy.Duration.from_sec(int(timeout))) if not finished_within_time:
exit(1) joint_names = ('shoulder_pitch_joint', 'shoulder_pan_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_pitch_joint', 'wrist_roll_joint') # pose that should be executed before anything else to allow opening gripper joint_positions = (-1.760, -1.760, 0.659, -0.795, -2.521, 0.619, -0.148) if not move_arm_to_grasping_joint_pose(joint_names, joint_positions): exit(1) posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction) posture_controller.wait_for_server() rospy.loginfo('connected to gripper posture controller') pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE posture_controller.send_goal(pg) posture_controller.wait_for_result() # define allowed contacts table = segmentation_result.table table_contact = AllowedContactSpecification() table_contact.name = coll_map_res.collision_support_surface_name table_contact.shape = Shape(type=Shape.BOX, dimensions=[abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), 1e-6]) table_contact.pose_stamped = table.pose
2 - CancelGoal 3 - Exit ''' def done_cb(terminal_state, result): print "DONE:", terminal_state, result def active_cb(): print "GOAL RCV" if __name__ == "__main__": rospy.init_node("nav_to_client") ac_ = SimpleActionClient("nav_to_node_action", NavToAction) ac_.wait_for_server() print 'sever connected!' help() while not rospy.is_shutdown(): cmd = raw_input('cmd:') if cmd == '1': x = input('target_x:') y = input('target_y:') yaw = input('yaw:') g = NavToActionGoal() q = tf.transformations.quaternion_from_euler(0, 0, yaw) g.goal.navgoal.pose.position.x = x g.goal.navgoal.pose.position.y = y g.goal.navgoal.pose.orientation.z = q[2] g.goal.navgoal.pose.orientation.w = q[3]
def __init__(self, context): self.prompt_width = 170 self.input_width = 250 super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._sound_client = SoundClient() #find relative path for files to load self.local_dir = os.path.dirname(__file__) self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/') if not os.path.isdir(self.dir): os.makedirs(self.dir) #need to add any additional subfolders as needed if not os.path.isdir(self.dir + 'animations/'): os.makedirs(self.dir + 'animations/') # Creates a subscriber to the ROS topic, having msg type SoundRequest rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) # Code used for saving/ loading arm poses for the robot switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) 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.all_joint_names = [] self.all_joint_poses = [] # Hash tables storing the name of the pose and the # associated positions for that pose, initially empty self.saved_l_poses = {} self.saved_r_poses = {} # Hash tables for storing name of animations and the associated pose list self.saved_animations = {} self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) #parsing for animations dir = os.path.dirname(__file__) qWarning(dir) filename = os.path.join(self.dir, 'animations/') ani_path = filename ani_listing = os.listdir(ani_path) for infile in ani_listing: pose_left = [] pose_right = [] left_gripper_states = [] right_gripper_states = [] line_num = 1 for line in fileinput.input(ani_path + infile): if (line_num % 2 == 1): pose = [float(x) for x in line.split()] pose_left.append(pose[:len(pose) / 2]) pose_right.append(pose[len(pose) / 2:]) else: states = line.split() left_gripper_states.append(states[0]) right_gripper_states.append(states[1]) line_num += 1 self.saved_animations[os.path.splitext(infile)[0]] = Quad( pose_left, pose_right, left_gripper_states, right_gripper_states) # 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() # Navigation functionality initialization self.roomNav = RoomNavigator() self._tf_listener = TransformListener() self.animPlay = AnimationPlayer(None, None, None, None) # Detection and pickup functionality self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav, self.animPlay) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) # Create a large vertical box that is top aligned large_box = QtGui.QVBoxLayout() large_box.setAlignment(QtCore.Qt.AlignTop) large_box.setMargin(0) large_box.addItem(QtGui.QSpacerItem(10, 0)) # Buttons for controlling the head of the robot head_box = QtGui.QHBoxLayout() head_box.addItem(QtGui.QSpacerItem(230, 0)) head_box.addWidget(self.create_pressed_button('Head Up')) head_box.addStretch(1) large_box.addLayout(head_box) button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(80, 0)) button_box.addWidget(self.create_pressed_button('Head Turn Left')) button_box.addWidget(self.create_pressed_button('Head Down')) button_box.addWidget(self.create_pressed_button('Head Turn Right')) button_box.addStretch(1) button_box.setMargin(0) button_box.setSpacing(0) large_box.addLayout(button_box) # Shows what the robot says speech_box = QtGui.QHBoxLayout() self.speech_label = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speech_label.setPalette(palette) # speech_box.addItem(QtGui.QSpacerItem(100, 0)) #speech_box.addWidget(self.speech_label) # large_box.addLayout(speech_box) # Speak button speak_button_box = QtGui.QHBoxLayout() speech_prompt = QtGui.QLabel('Enter Speech Text:') speech_prompt.setFixedWidth(self.prompt_width) speak_button_box.addWidget(speech_prompt) robot_says = QtGui.QLineEdit(self._widget) robot_says.setFixedWidth(self.input_width) robot_says.textChanged[str].connect(self.onChanged) # speak_button_box.addWidget(robot_says) speak_button_box.addWidget(self.create_button('Speak')) speak_button_box.addStretch(1) large_box.addLayout(speak_button_box) large_box.addItem(QtGui.QSpacerItem(0, 50)) # Buttons for arm poses pose_button_box1 = QtGui.QHBoxLayout() pose_button_box1.addItem(QtGui.QSpacerItem(150, 0)) pose_button_box1.addWidget(self.create_button('Relax Left Arm')) pose_button_box1.addWidget(self.create_button('Relax Right Arm')) pose_button_box1.addStretch(1) large_box.addLayout(pose_button_box1) # Buttons for grippers gripper_button_box = QtGui.QHBoxLayout() gripper_button_box.addItem(QtGui.QSpacerItem(150, 0)) gripper_button_box.addWidget(self.create_button('Open Left Gripper')) gripper_button_box.addWidget(self.create_button('Open Right Gripper')) gripper_button_box.addStretch(1) large_box.addLayout(gripper_button_box) large_box.addItem(QtGui.QSpacerItem(0, 25)) # Buttons for animation animation_box = QtGui.QHBoxLayout() play_anim_label = QtGui.QLabel('Select Animation:') play_anim_label.setFixedWidth(self.prompt_width) animation_box.addWidget(play_anim_label) self.saved_animations_list = QtGui.QComboBox(self._widget) animation_box.addWidget(self.saved_animations_list) pose_time_label = QtGui.QLabel('Duration(sec):') pose_time_label.setFixedWidth(100) animation_box.addWidget(pose_time_label) self.pose_time = QtGui.QLineEdit(self._widget) self.pose_time.setFixedWidth(50) self.pose_time.setText('2.0') animation_box.addWidget(self.pose_time) animation_box.addWidget(self.create_button('Play Animation')) animation_box.addStretch(1) large_box.addLayout(animation_box) animation_box2 = QtGui.QHBoxLayout() animation_name_label = QtGui.QLabel('Enter Animation Name:') animation_name_label.setFixedWidth(self.prompt_width) animation_box2.addWidget(animation_name_label) self.animation_name = QtGui.QLineEdit(self._widget) self.animation_name.setFixedWidth(self.input_width) animation_box2.addWidget(self.animation_name) animation_box2.addWidget(self.create_button('Save Animation')) animation_box2.addStretch(1) large_box.addLayout(animation_box2) animation_box3 = QtGui.QHBoxLayout() pose_name_label = QtGui.QLabel('Enter Pose Name:') pose_name_label.setFixedWidth(self.prompt_width) animation_box3.addWidget(pose_name_label) self.pose_name_temp = QtGui.QLineEdit(self._widget) self.pose_name_temp.setFixedWidth(self.input_width) animation_box3.addWidget(self.pose_name_temp) animation_box3.addWidget(self.create_button('Add Current Pose')) animation_box3.addStretch(1) large_box.addLayout(animation_box3) # Playing around with UI stuff play_box = QtGui.QHBoxLayout() pose_sequence_label = QtGui.QLabel('Current Pose Sequence:') pose_sequence_label.setFixedWidth(self.prompt_width) pose_sequence_label.setAlignment(QtCore.Qt.AlignTop) self.list_widget = QListWidget() self.list_widget.setDragDropMode(QAbstractItemView.InternalMove) self.list_widget.setMaximumSize(self.input_width, 200) play_box.addWidget(pose_sequence_label) play_box.addWidget(self.list_widget) play_box.addStretch(1) large_box.addLayout(play_box) large_box.addItem(QtGui.QSpacerItem(0, 50)) # Buttons for first row of base controls first_base_button_box = QtGui.QHBoxLayout() first_base_button_box.addItem(QtGui.QSpacerItem(70, 0)) first_base_button_box.addWidget( self.create_pressed_button('Rotate Left')) first_base_button_box.addWidget(self.create_pressed_button('^')) first_base_button_box.addWidget( self.create_pressed_button('Rotate Right')) first_base_button_box.addStretch(1) large_box.addLayout(first_base_button_box) # Buttons for second row of base controls second_base_button_box = QtGui.QHBoxLayout() second_base_button_box.addItem(QtGui.QSpacerItem(70, 0)) second_base_button_box.addWidget(self.create_pressed_button('<')) second_base_button_box.addWidget(self.create_pressed_button('v')) second_base_button_box.addWidget(self.create_pressed_button('>')) second_base_button_box.addWidget(self.create_button('Move to Bin')) second_base_button_box.addWidget(self.create_button('Object Detect')) second_base_button_box.addWidget(self.create_button('Clean Room')) second_base_button_box.addStretch(1) large_box.addLayout(second_base_button_box) # Animation related items to store intermediate pose co-ordinates to save self.animation_map = {} self.create_state = False self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) # Look straight and down when launched self.head_x = 1.0 self.head_y = 0.0 self.head_z = 0.5 self.head_action(self.head_x, self.head_y, self.head_z) # Set grippers to closed on initialization self.left_gripper_state = '' self.right_gripper_state = '' self.gripper_action('l', 0.0) self.gripper_action('r', 0.0) # Lab 6 self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Saved states for poses saved_pose_box = QtGui.QHBoxLayout() self.saved_left_poses = QtGui.QLabel('') self.saved_right_poses = QtGui.QLabel('') saved_pose_box.addWidget(self.saved_left_poses) saved_pose_box.addWidget(self.saved_right_poses) large_box.addLayout(saved_pose_box) # Preload the map of animations self.saved_animations_list.addItems(self.saved_animations.keys()) # Move the torso all the way down # self.torso_down(True) # Autonomous navigation stuff ''' self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)), Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)), Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)), Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)), Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)), Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)), Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)), Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))] ''' self.locations = [ Pose(Point(2.04748392105, 2.04748010635, 0.000), Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)), Pose(Point(2.34193611145, 1.43208932877, 0), Quaternion(0, 0, -0.841674385779, 0.539985396398)), Pose(Point(3.43202018738, -0.258297920227, 0.000), Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)), Pose(Point(0.931655406952, -1.96435832977, 0.000), Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)), Pose(Point(-0.369112968445, 0.0330476760864, 0.000), Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453)) ] self.index = 1 rospy.loginfo("Completed GUI initialization")
if __name__ == '__main__': try: if len(sys.argv) > 1: app_name = sys.argv[1] rospy.init_node(app_name + '_key_ctrl') else: rospy.init_node('app_key_ctrl_smach') # window.mainloop() will block, ignoring Ctrl+C signal, so we stop it manually on ROS shutdown rospy.on_shutdown(quit_tk) # Creates the SimpleActionClient, passing the type of the action # (UserCommandAction) to the constructor. client = SimpleActionClient('user_commands_action_server', thorp_msgs.UserCommandAction) # Waits until the action server has started up and started listening for goals. client.wait_for_server() # Create a simple GUI to show available commands and capture keyboard inputs window = tk.Tk() window.geometry(rospy.get_param('~window_geometry', '300x200')) window.wm_title(rospy.get_param('~window_caption', 'User input')) text = tk.Text(window, background='black', foreground='white', font=(rospy.get_param('~text_font', 'Comic Sans MS'), rospy.get_param('~font_size', 12))) valid_cmds = rospy.get_param('~valid_commands', [])
def run(self): self.event_type = 'uncontrollable' # Event type selected on Radio self.enabled_e = [] # List of enabled events self.param = [] # Parameters of the event trace = Thread( target=self.events_trace) # Thread for the tracer monitor trace.start() # Start event tracer as a thread while True: #Get data from the Window event, values = self.window.Read(timeout=10) if event in (None, 'Cancel'): # If user closes window or clicks cancel print('\nCLOSING EVENT INTERFACE ...\n') break # New event occured on the Prodct System if self.new_trace == True: self.new_trace = False #Update tracer screen if self.trace.tail(1).index[0] > 0: if self.__events[self.trace.tail(1) ['event'].values[0]].is_controllable(): color = 'blue' else: color = 'red' text = self.trace.tail(1).drop( columns=['enabled_events', 'states']).to_string( header=False, justify='left') self.window['tracer'].print(text, text_color=color) #Update the Automaton Image try: self.window.Element("_IMAGE_").update( filename="output/" + values['option'] + ".png") except: pass if event == 'option': #Update the Automaton Image try: self.window.Element("_IMAGE_").update( filename="output/" + values['option'] + ".png") except: pass elif event == 'controllable': #Show list of enabled controllable events self.window.Element('selected_event').update( values=self.enabled_e) self.event_type = 'controllable' elif event == 'uncontrollable': #Show list of uncontrollable events self.window.Element('selected_event').update( values=self.__not_cont_e) self.event_type = 'uncontrollable' if event == 'trigger': # An event is triggered if values['controllable'] == True: trigger_event( values['selected_event'], self.param ) # Call the execution of the controllable event else: # Trigger uncontrollable events event = values['selected_event'] # Translate non-controllable events to low-level call ll_event = self.translation_table[( self.translation_table['high-level'] == event )]['low-level'].array[0] topic = self.translation_table[( self.translation_table['high-level'] == event )]['topic'].array[0] # Fake uncontrollable events if 'erro' in ll_event: if 'maneuvers/out' in topic: # Fake maneuver error move_base_client = SimpleActionClient( "{}move_base".format(rospy.get_namespace()), MoveBaseAction) # Get move_base service move_base_client.wait_for_server() move_base_client.cancel_all_goals( ) # Cancel the current motion elif ('gas_sensor/out' in topic) or ('victim_sensor/out' in topic): # Fake sensor error topic = topic.replace( '/out', '/in') # Get sensor/in topic to fake a failure pub = rospy.Publisher("/{}".format(topic), events_message, queue_size=10) msg = events_message() msg.event = ll_event pub.publish(msg) # Publish to the sensor topic elif 'bat_' in event: # Fake battery state change topic = topic.replace( '/out', '/in') # Get battery_monitor/in topic pub = rospy.Publisher("/{}".format(topic), events_message, queue_size=10) msg = events_message() msg.event = ll_event if event == 'bat_OK': msg.param.append( 60.0 ) # At level = 60 the system consider bat_OK elif event == 'bat_L': msg.param.append( 30.0 ) # At level = 30 the system consider bat_L elif event == 'bat_LL': msg.param.append( 9.0) # At level = 9 the system consider bat_LL pub.publish( msg) # Publish to the battery_monitor topic elif 'failure' in ll_event: # Fake failures topic = topic.replace( '/out', '/in') # Get failures_monitor/in topic pub = rospy.Publisher("/{}".format(topic), events_message, queue_size=10) msg = events_message() msg.event = ll_event pub.publish( msg) # Publish to the failures_monitor topic elif 'ihm' in topic: # Fake commander comands pub = rospy.Publisher("/{}".format(topic), events_message, queue_size=10) msg = events_message() msg.event = ll_event pub.publish(msg) # Publish to the IHM/out topic self.param = [] # Clear param self.window['param_list'].Update( values=self.param) # Clear param screen elif event == 'refresh': self.window['tracer'].update('') #Refresh tracer if not self.trace.empty: for i in self.trace.iloc[1:].index: if self.__events[self.trace.at[ i, 'event']].is_controllable(): color = 'blue' else: color = 'red' text = self.trace.loc[[i]].drop( columns=['enabled_events', 'states']).to_string( header=False, justify='left') self.window['tracer'].print(text, text_color=color) elif event == 'save': # Save content of tracer into a csv file filename = values['save'] if filename: if '.' not in filename: filename += ".csv" if '.csv' in filename: self.trace.drop(columns=['enabled_events', 'states' ]).to_csv(filename) else: sg.Popup('Wrong file extension!', title='Saving failure!') self.window['save'].update(value='') elif event == 'add_param': # Add a new item as parameter for the event if values['new_param']: self.param.append(eval(values['new_param'])) self.window['new_param'].update('') self.window['param_list'].Update(values=self.param) elif event == 'remove_param': # Remove an item from the list of parameters if self.window['param_list'].GetIndexes(): item = self.window['param_list'].GetIndexes()[0] self.param.pop(item) self.window['param_list'].Update(values=self.param) self.window.Close()
def __init__(self,parent,id): the_size=(700, 570) wx.Frame.__init__(self,parent,id,'Elfin Control Panel',pos=(250,100)) self.panel=wx.Panel(self) font=self.panel.GetFont() font.SetPixelSize((12, 24)) self.panel.SetFont(font) self.listener = tf.TransformListener() self.robot=moveit_commander.RobotCommander() self.scene=moveit_commander.PlanningSceneInterface() self.group=moveit_commander.MoveGroupCommander('elfin_arm') self.controller_ns='elfin_arm_controller/' self.elfin_driver_ns='elfin_ros_control/elfin/' self.elfin_basic_api_ns='elfin_basic_api/' self.joint_names=rospy.get_param(self.controller_ns+'joints', []) self.ref_link_name=self.group.get_planning_frame() self.end_link_name=self.group.get_end_effector_link() self.ref_link_lock=threading.Lock() self.end_link_lock=threading.Lock() self.js_display=[0]*6 # joint_states self.jm_button=[0]*6 # joints_minus self.jp_button=[0]*6 # joints_plus self.js_label=[0]*6 # joint_states self.ps_display=[0]*6 # pcs_states self.pm_button=[0]*6 # pcs_minus self.pp_button=[0]*6 # pcs_plus self.ps_label=[0]*6 # pcs_states self.display_init() self.key=[] btn_height=390 btn_lengths=[] self.power_on_btn=wx.Button(self.panel, label=' Servo On ', name='Servo On', pos=(20, btn_height)) btn_lengths.append(self.power_on_btn.GetSize()[0]) btn_total_length=btn_lengths[0] self.power_off_btn=wx.Button(self.panel, label=' Servo Off ', name='Servo Off') btn_lengths.append(self.power_off_btn.GetSize()[0]) btn_total_length+=btn_lengths[1] self.reset_btn=wx.Button(self.panel, label=' Clear Fault ', name='Clear Fault') btn_lengths.append(self.reset_btn.GetSize()[0]) btn_total_length+=btn_lengths[2] self.home_btn=wx.Button(self.panel, label='Home', name='home_btn') btn_lengths.append(self.home_btn.GetSize()[0]) btn_total_length+=btn_lengths[3] self.stop_btn=wx.Button(self.panel, label='Stop', name='Stop') btn_lengths.append(self.stop_btn.GetSize()[0]) btn_total_length+=btn_lengths[4] btn_interstice=(550-btn_total_length)/4 btn_pos_tmp=btn_lengths[0]+btn_interstice+20 self.power_off_btn.SetPosition((btn_pos_tmp, btn_height)) btn_pos_tmp+=btn_lengths[1]+btn_interstice self.reset_btn.SetPosition((btn_pos_tmp, btn_height)) btn_pos_tmp+=btn_lengths[2]+btn_interstice self.home_btn.SetPosition((btn_pos_tmp, btn_height)) btn_pos_tmp+=btn_lengths[3]+btn_interstice self.stop_btn.SetPosition((btn_pos_tmp, btn_height)) self.servo_state_label=wx.StaticText(self.panel, label='Servo state:', pos=(590, btn_height-10)) self.servo_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', pos=(600, btn_height+10)) self.servo_state=bool() self.fault_state_label=wx.StaticText(self.panel, label='Fault state:', pos=(590, btn_height+60)) self.fault_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', pos=(600, btn_height+80)) self.fault_state=bool() self.reply_show_label=wx.StaticText(self.panel, label='Result:', pos=(20, btn_height+120)) self.reply_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY), value='', size=(670, 30), pos=(20, btn_height+140)) link_textctrl_length=(btn_pos_tmp-40)/2 self.ref_links_show_label=wx.StaticText(self.panel, label='Ref. link:', pos=(20, btn_height+60)) self.ref_link_show=wx.TextCtrl(self.panel, style=(wx.TE_READONLY), value=self.ref_link_name, size=(link_textctrl_length, 30), pos=(20, btn_height+80)) self.end_link_show_label=wx.StaticText(self.panel, label='End link:', pos=(link_textctrl_length+30, btn_height+60)) self.end_link_show=wx.TextCtrl(self.panel, style=(wx.TE_READONLY), value=self.end_link_name, size=(link_textctrl_length, 30), pos=(link_textctrl_length+30, btn_height+80)) self.set_links_btn=wx.Button(self.panel, label='Set links', name='Set links') self.set_links_btn.SetPosition((btn_pos_tmp, btn_height+75)) # the variables about velocity scaling velocity_scaling_init=rospy.get_param(self.elfin_basic_api_ns+'velocity_scaling', default=0.4) default_velocity_scaling=str(round(velocity_scaling_init, 2)) self.velocity_setting_label=wx.StaticText(self.panel, label='Velocity Scaling', pos=(20, btn_height-70)) self.velocity_setting=wx.Slider(self.panel, value=int(velocity_scaling_init*100), minValue=1, maxValue=100, style = wx.SL_HORIZONTAL, size=(500, 30), pos=(45, btn_height-50)) self.velocity_setting_txt_lower=wx.StaticText(self.panel, label='1%', pos=(20, btn_height-45)) self.velocity_setting_txt_upper=wx.StaticText(self.panel, label='100%', pos=(550, btn_height-45)) self.velocity_setting_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER|wx.TE_READONLY), value=default_velocity_scaling, pos=(600, btn_height-55)) self.velocity_setting.Bind(wx.EVT_SLIDER, self.velocity_setting_cb) self.teleop_api_dynamic_reconfig_client=dynamic_reconfigure.client.Client(self.elfin_basic_api_ns, config_callback=self.basic_api_reconfigure_cb) self.dlg=wx.Dialog(self.panel, title='messag') self.dlg.Bind(wx.EVT_CLOSE, self.closewindow) self.dlg_panel=wx.Panel(self.dlg) self.dlg_label=wx.StaticText(self.dlg_panel, label='hello', pos=(15, 15)) self.set_links_dlg=wx.Dialog(self.panel, title='Set links', size=(400, 100)) self.set_links_dlg_panel=wx.Panel(self.set_links_dlg) self.sld_ref_link_show=wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER, value='', pos=(20, 20), size=(link_textctrl_length, 30)) self.sld_end_link_show=wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER, value='', pos=(20, 70), size=(link_textctrl_length, 30)) self.sld_set_ref_link_btn=wx.Button(self.set_links_dlg_panel, label='Update ref. link', name='Update ref. link') self.sld_set_ref_link_btn.SetPosition((link_textctrl_length+30, 15)) self.sld_set_end_link_btn=wx.Button(self.set_links_dlg_panel, label='Update end link', name='Update end link') self.sld_set_end_link_btn.SetPosition((link_textctrl_length+30, 65)) self.set_links_dlg.SetSize((link_textctrl_length+self.sld_set_ref_link_btn.GetSize()[0]+50, 120)) self.call_teleop_joint=rospy.ServiceProxy(self.elfin_basic_api_ns+'joint_teleop', SetInt16) self.call_teleop_joint_req=SetInt16Request() self.call_teleop_cart=rospy.ServiceProxy(self.elfin_basic_api_ns+'cart_teleop', SetInt16) self.call_teleop_cart_req=SetInt16Request() self.call_teleop_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop', SetBool) self.call_teleop_stop_req=SetBoolRequest() self.call_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop', SetBool) self.call_stop_req=SetBoolRequest() self.call_stop_req.data=True self.stop_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_stop, rq=self.call_stop_req : self.call_set_bool_common(evt, cl, rq)) self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool) self.call_reset_req=SetBoolRequest() self.call_reset_req.data=True self.reset_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_reset, rq=self.call_reset_req : self.call_set_bool_common(evt, cl, rq)) self.call_power_on=rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool) self.call_power_on_req=SetBoolRequest() self.call_power_on_req.data=True self.power_on_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_power_on, rq=self.call_power_on_req : self.call_set_bool_common(evt, cl, rq)) self.call_power_off=rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool) self.call_power_off_req=SetBoolRequest() self.call_power_off_req.data=True self.power_off_btn.Bind(wx.EVT_BUTTON, lambda evt, cl=self.call_power_off, rq=self.call_power_off_req : self.call_set_bool_common(evt, cl, rq)) self.call_move_homing=rospy.ServiceProxy(self.elfin_basic_api_ns+'home_teleop', SetBool) self.call_move_homing_req=SetBoolRequest() self.call_move_homing_req.data=True self.home_btn.Bind(wx.EVT_LEFT_DOWN, lambda evt, cl=self.call_move_homing, rq=self.call_move_homing_req : self.call_set_bool_common(evt, cl, rq)) self.home_btn.Bind(wx.EVT_LEFT_UP, lambda evt, mark=100: self.release_button(evt, mark) ) self.call_set_ref_link=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_reference_link', SetString) self.call_set_end_link=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_end_link', SetString) self.set_links_btn.Bind(wx.EVT_BUTTON, self.show_set_links_dialog) self.sld_set_ref_link_btn.Bind(wx.EVT_BUTTON, self.update_ref_link) self.sld_set_end_link_btn.Bind(wx.EVT_BUTTON, self.update_end_link) self.sld_ref_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_ref_link) self.sld_end_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_end_link) self.action_client=SimpleActionClient(self.controller_ns+'follow_joint_trajectory', FollowJointTrajectoryAction) self.action_goal=FollowJointTrajectoryGoal() self.action_goal.trajectory.joint_names=self.joint_names self.SetMinSize(the_size) self.SetMaxSize(the_size)
def __init__(self, script_path): rospy.init_node('show_time') rospy.on_shutdown(self.cleanup) # Initialize a number of parameters and variables for nav locations # Set the default TTS voice to use self.voice = rospy.get_param("~voice", "voice_en1_mbrola") self.robot = rospy.get_param("~robot", "robbie") self.start = (Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018))) self.left = (Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401))) self.right = (Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480))) self.auto_dock = (Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000))) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() # Create the sound client object self.soundhandle = SoundClient() # Wait a moment to let the client connect to the # sound_play server rospy.sleep(2) # Make sure any lingering sound_play processes are stopped. self.soundhandle.stopAll() # Set the wave file path if used self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds") #define afternoon and morning self.noon = strftime("%p:", localtime()) if self.noon == "AM:": self.noon1 = "Goood Morning " else: self.noon1 = "Good After Noon " self.local = strftime("%H:%M:", localtime()) #local time self.local = strftime("%H:%M:", localtime()) self.soundhandle.say( self.noon1 + self.robot + " is on line" + " the time is " + self.local, self.voice) rospy.sleep(6) self.move_to(self.start) self.soundhandle.say( "Welcome to SHOW time. This is where I get to demonstrate my capabilities", self.voice) rospy.sleep(5) self.soundhandle.say("I can move to my left", self.voice) rospy.sleep(2) self.move_to(self.left) self.soundhandle.say("now back to the start position", self.voice) rospy.sleep(2) self.move_to(self.start) self.soundhandle.say("I can move to my right", self.voice) rospy.sleep(2) self.move_to(self.right) self.soundhandle.say("And again back to the start position", self.voice) rospy.sleep(2) self.move_to(self.start) rospy.sleep(2) self.soundhandle.say("Thank you for your time ", self.voice) rospy.sleep(2) #rospy.sleep(2) self.move_to(self.auto_dock)
def __init__(self, controller_name): topic = ''.join((controller_name, '/joint_trajectory_action')) self.client = SimpleActionClient(topic, JointTrajectoryAction) rospy.loginfo('waiting for action client: %s' % topic) self.client.wait_for_server()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() #move_base action self.fridge = (Pose(Point(X_FRIDGE, Y_FRIDGE, 0.0), Quaternion(0.0, 0.0, 0, 1))) #location of the beer self.person = (Pose(Point(X_PERSON, Y_PERSON, 0.0), Quaternion(0.0, 0.0, 0, 1))) #person requesting the beer self.station = (Pose(Point(0.5, 0.0, 0.0), Quaternion(0.0, 0.0, 0, 1))) #person requesting the beer self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) left_arm = MoveGroupCommander('left_arm') # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' person1_id = 'person1' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) scene.remove_world_object(person1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() left_arm.set_named_target('left_start') left_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] person1_size = [0.3, 0.7, 0.01] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = X_FRIDGE + 0.55 table_pose.pose.position.y = Y_FRIDGE + 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = X_FRIDGE + 0.55 box1_pose.pose.position.y = Y_FRIDGE + -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = X_FRIDGE + 0.54 box2_pose.pose.position.y = Y_FRIDGE + 0.13 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) #add the person to the scene person1_pose = PoseStamped() person1_pose.header.frame_id = REFERENCE_FRAME person1_pose.pose.position.x = X_PERSON + 0.54 person1_pose.pose.position.y = Y_PERSON + 0.13 person1_pose.pose.position.z = table_ground + table_size[ 2] + person1_size[2] / 2.0 person1_pose.pose.orientation.w = 1.0 scene.add_box(person1_id, person1_pose, person1_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = X_FRIDGE + 0.50 target_pose.pose.position.y = Y_FRIDGE + 0.0 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) self.setColor(person1_id, 0.8, 0, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = X_PERSON + 0.50 place_pose.pose.position.y = Y_PERSON + -0.25 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 #move to target self.move_to(self.fridge) # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- right_arm.set_named_target('r_travel') right_arm.go() self.move_to(self.person) #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) #move to station self.move_to(self.station) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
#! /usr/bin/env python import sys import rospy from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from diagnostic_msgs.msg import KeyValue from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal if __name__ == "__main__": rospy.init_node("place_object_client_tester") client = SimpleActionClient("place_object_server", GenericExecuteAction) client.wait_for_server() goal = GenericExecuteGoal() if len(sys.argv) > 1: location = str(sys.argv[1]).upper() goal.parameters.append(KeyValue(key="location", value=location)) rospy.loginfo("Sending following goal to place object server") rospy.loginfo(goal) client.send_goal(goal) timeout = 15.0 finished_within_time = client.wait_for_result( rospy.Duration.from_sec(int(timeout))) if not finished_within_time: client.cancel_goal()
def execute_inner(self, userdata): global ms self.last_mv = rospy.Time.now() rospy.loginfo('Beginning Exploration') client = SimpleActionClient('explore_server', ExploreTaskAction) rospy.loginfo('WAITING FOR EXPLORE SERVER...') client.wait_for_server() rospy.loginfo('EXPLORE SERVER IS NOW UP!') boundary = PolygonStamped() boundary.header.frame_id = "map" boundary.header.stamp = rospy.Time.now() r = userdata.boundary / 2.0 rospy.loginfo('boundary : {}'.format(r)) x, y, _ = userdata.initial_point boundary.polygon.points.append(Point(x + r, y + r, 0)) boundary.polygon.points.append(Point(x - r, y + r, 0)) boundary.polygon.points.append(Point(x - r, y - r, 0)) boundary.polygon.points.append(Point(x + r, y - r, 0)) center = PointStamped() center.header.frame_id = "map" center.header.stamp = rospy.Time.now() center.point.x = x center.point.y = y center.point.z = 0.0 goal = ExploreTaskGoal() goal.explore_boundary = boundary goal.explore_center = center client.send_goal(goal) while True: if rospy.is_shutdown(): exit() if client.wait_for_result( rospy.Duration(0.3)): # 0.3 sec. timeout to check for cans # The exploration node has finished res = client.get_state() rospy.loginfo('EXPLORE SERVER STATE:{}'.format(res)) if res == 3: ## SUCCEEDED # if exploration is complete... userdata.boundary += 1.0 # explore a larger area return 'succeeded' # finished! yay! else: print client.get_goal_status_text() print 'explore server failed : {}'.format(res) # when explore server gives up, can't explore return 'stuck' now = rospy.Time.now() if self.objective == 'discovery': t = ms.dis_data.time p = ms.dis_pt() elif self.objective == 'delivery': t = ms.del_data.time p = ms.del_pt() else: rospy.logerr('Invalid objective passed to Explore state') return 'aborted' # if we're here, exploration is not complete yet... if t != None: # check initialized dt = (now - t).to_sec() discovered = (dt < 2.0) # last seen within the last 2 seconds if discovered: # if can was found ... client.cancel_all_goals() userdata.destination = p return 'discovered' # more than 20 seconds have passed while completely still # we're probably stuck if (now - self.last_mv).to_sec() > 20.0: print 'haven\'t been moving for a while!' client.cancel_all_goals() return 'stuck' # bad name... "stuck" would be better
def do_it(msg): rospy.loginfo('We are DOING IT!') # connect to cluster bounding box finding service rospy.loginfo('waiting for find_cluster_bounding_box service') rospy.wait_for_service('/find_cluster_bounding_box') find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox) rospy.loginfo('connected to find_cluster_bounding_box') target_location = msg.target_location t_p1 = msg.object_bbox.points[0] t_p2 = msg.object_bbox.points[1] t_p1.y, t_p2.y = t_p2.y, t_p1.y reset = ResetRobot() reset.execute() rospy.loginfo('Robot reset') segment = SegmentScene() res = segment.execute() if not res: exit(1) rospy.loginfo('Scene segmented') # find a cluster that intersects with a provided bounding box for idx, cluster in enumerate(res[1]['segmentation_result'].clusters): bbox_response = find_bounding_box_srv(cluster) if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS: rospy.logwarn('unable to find for cluster %d bounding box' % idx) continue o_center = bbox_response.pose.pose.position o_p1 = Point32(o_center.x - bbox_response.box_dims.x, o_center.y - bbox_response.box_dims.y, o_center.z) o_p2 = Point32(o_center.x + bbox_response.box_dims.x, o_center.y + bbox_response.box_dims.y, o_center.z) print idx, t_p1, t_p2, o_p1, o_p2 if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y) and (o_p1.x < t_p2.x and o_p1.y < t_p2.y)): closest_index = idx break grasp_client = SimpleActionClient('grasp_object', GraspObjectAction) grasp_client.wait_for_server() grasp_goal = GraspObjectGoal() grasp_goal.category_id = -1 grasp_goal.graspable_object = res[0].graspable_objects[closest_index] grasp_goal.collision_object_name = res[0].collision_object_names[ closest_index] grasp_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = grasp_client.send_goal_and_wait(grasp_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to grasp') reset.execute() exit(1) rospy.loginfo('Grasped an object') lift_client = SimpleActionClient('lift_object', LiftObjectAction) lift_client.wait_for_server() lift_goal = LiftObjectGoal() lift_goal.category_id = 0 lift_goal.graspable_object = res[0].graspable_objects[closest_index] lift_goal.collision_object_name = res[0].collision_object_names[ closest_index] lift_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = lift_client.send_goal_and_wait(lift_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to lift') exit(1) rospy.loginfo('Lifted an object') pudown_client = SimpleActionClient('put_down_at', PutDownAtAction) pudown_client.wait_for_server() putdown_goal = PutDownAtGoal() putdown_goal.category_id = -1 putdown_goal.graspable_object = res[0].graspable_objects[closest_index] putdown_goal.collision_object_name = res[0].collision_object_names[ closest_index] putdown_goal.collision_support_surface_name = res[ 0].collision_support_surface_name putdown_goal.target_location = Point( target_location.x, target_location.y, target_location.z) #Point(0.468, 0.226, 0.251) result = pudown_client.send_goal_and_wait(putdown_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to put down') reset.execute() exit(1) rospy.loginfo('Pu down the object') reset.execute()
def __init__(self, timeout=5.0): self.__ac = SimpleActionClient("/talk_request_action", TalkRequestAction) self.__ac.wait_for_server(rospy.Duration(timeout))
#! /usr/bin/env python import sys import rospy from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from diagnostic_msgs.msg import KeyValue from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal if __name__ == "__main__": rospy.init_node("unstage_object_client_tester") client = SimpleActionClient("unstage_object_server", GenericExecuteAction) client.wait_for_server() if len(sys.argv) > 1: platform = str(sys.argv[1]).upper() if len(sys.argv) > 2: obj = str(sys.argv[2]).upper() else: obj = "M20" else: platform = "PLATFORM_MIDDLE" goal = GenericExecuteGoal() goal.parameters.append(KeyValue(key="platform", value=platform)) goal.parameters.append(KeyValue(key="object", value=obj)) rospy.loginfo("Sending following goal to unstage object server") rospy.loginfo(goal)
import rospy from actionlib import SimpleActionClient, GoalStatus from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal, PlayMotionResult PLAY_MOTION_AS = "/play_motion" from helper_functions import goal_status_dict def test_done_cb(): print "We got a done!" if __name__ == '__main__': rospy.init_node("test_donecb") rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'") play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction) play_motion_as.wait_for_server() rospy.loginfo("Connected.") play_motion_as.done_cb = test_done_cb # This is never called pm_goal = PlayMotionGoal("home", False, 0) play_motion_as.send_goal(pm_goal) rospy.loginfo("Sent goal.") #play_motion_as.wait_for_result() # I thought this was necessary to actually start the goal, but it's not done = False while not done: state = play_motion_as.get_state() rospy.loginfo("State: " + str(state) + " Which is: " + goal_status_dict[state]) if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED: done = True rospy.loginfo("Finished goal")
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5): """ :param name: 'left' or 'right' :param rate: Rate of the control loop for execution of motions :param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros" :param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros" :param default_kv_max: Default K maximum for velocity :param default_ka_max: Default K maximum for acceleration """ Limb.__init__(self, name) self._world = 'base' self.kv_max = default_kv_max self.ka_max = default_ka_max self._gripper = Gripper(name) self._rate = rospy.Rate(rate) self._tf_listener = TransformListener() self.recorder = Recorder(name) # Kinematics services: names and services (if relevant) self._kinematics_names = { 'fk': { 'ros': 'compute_fk' }, 'ik': { 'ros': 'compute_ik', 'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format( name), 'trac': 'trac_ik_{}'.format(name) } } self._kinematics_services = { 'fk': { 'ros': { 'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK), 'func': self._get_fk_ros }, 'kdl': { 'func': self._get_fk_pykdl }, 'robot': { 'func': self._get_fk_robot } }, 'ik': { 'ros': { 'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK), 'func': self._get_ik_ros }, 'robot': { 'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK), 'func': self._get_ik_robot }, 'trac': { 'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK), 'func': self._get_ik_trac }, 'kdl': { 'func': self._get_ik_pykdl } } } self._selected_ik = ik self._selected_fk = fk # Kinematics services: PyKDL self._kinematics_pykdl = baxter_kinematics(name) if self._selected_ik in self._kinematics_names['ik']: rospy.wait_for_service( self._kinematics_names['ik'][self._selected_ik]) if self._selected_fk in self._kinematics_names['fk']: rospy.wait_for_service( self._kinematics_names['fk'][self._selected_fk]) # Execution attributes rospy.Subscriber( '/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1) rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1) self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped self._stop_lock = Lock() action_server_name = "/robot/limb/{}/follow_joint_trajectory".format( self.name) self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction) self._display_traj = rospy.Publisher( "/move_group/display_planned_path", DisplayTrajectory, queue_size=1) self._gripper.calibrate() self.client.wait_for_server()
def __init__(self): # Retrieve params: #self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'object_8') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.08) self._arm_group = rospy.get_param('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: #self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: #self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = self._pose_coke_can #self._pose_place.position.x = self._pose_coke_can.position.x #self._pose_place.position.y = self._pose_coke_can.position.y - 0.10 #self._pose_place.position.z = self._pose_coke_can.position.z + 0.08 #self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully')
def main(): #Find an object to pick up psi = get_planning_scene_interface() psi.reset() rospy.loginfo('Doing detection') detector = TablewareDetection() det = detector.detect_objects(add_objects_as_mesh=False) if not det.pickup_goals: rospy.loginfo('Nothing to pick up!') return psi.reset() #DARRT action client client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) rospy.loginfo('Waiting for DARRT action') client.wait_for_server() rospy.loginfo('Found DARRT action') #DARRTGoal goal = DARRTGoal() #pickup goal (from the tabletop detection) goal.pickup_goal = det.pickup_goals[0] goal.pickup_goal.arm_name = 'right_arm' #world interface to tell us whether we're using the #map or odom_combined frame wi = WorldInterface() #place goal (move far away) place_pose_stamped = PoseStamped() place_pose_stamped.header.frame_id = wi.world_frame place_pose_stamped.pose.position.x = -2 place_pose_stamped.pose.position.y = 0 place_pose_stamped.pose.position.z = 0.85 place_pose_stamped.pose.orientation.w = 1.0 goal.place_goal = PlaceGoal( goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name=det.table_name, collision_object_name=goal.pickup_goal.collision_object_name) goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT] goal.planning_time = 30 goal.tries = 3 goal.debug_level = 1 goal.object_sampling_fraction = 0.5 #Send the goal to the action rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') #Check the result result = client.get_result() if result.error_code != result.SUCCESS: rospy.logerr('Planning failed. Error code: ' + str(result.error_code)) return rospy.loginfo('Planning succeeded!') #at this point the planning is done. #now we execute and visualize the plan #visualize trajectory pub = rospy.Publisher('darrt_trajectory', MarkerArray) marray = get_trajectory_markers(result.primitive_trajectories) for i in range(10): pub.publish(marray) rospy.sleep(0.05) #Executor is a Python class for executing DARRT plans executor = Executor() #execute trajectory rospy.loginfo('Press enter to execute') raw_input() executor.execute_trajectories(result) rospy.loginfo('Successfully executed!')
return (tcmpr, res['cluster_information']) if __name__ == '__main__': rospy.init_node('grab_and_run') reset = ResetRobot() reset.execute() segment = SegmentScene() res = segment.execute() if not res: exit(1) closest_index = res[1][0][0] grasp_client = SimpleActionClient('grasp_object', GraspObjectAction) grasp_client.wait_for_server() grasp_goal = GraspObjectGoal() grasp_goal.category_id = -1 grasp_goal.graspable_object = res[0].graspable_objects[closest_index] grasp_goal.collision_object_name = res[0].collision_object_names[ closest_index] grasp_goal.collision_support_surface_name = res[ 0].collision_support_surface_name result = grasp_client.send_goal_and_wait(grasp_goal) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to grasp') reset.execute()
def __init__(self): self.object_detector = ObjectDetector() # connect to collision map processing service rospy.loginfo('waiting for tabletop_collision_map_processing service') rospy.wait_for_service( '/tabletop_collision_map_processing/tabletop_collision_map_processing' ) self.collision_map_processing_srv = rospy.ServiceProxy( '/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing) rospy.loginfo('connected to tabletop_collision_map_processing service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_grasp_action') self.posture_controller = SimpleActionClient( '/wubble_gripper_grasp_action', GraspHandPostureExecutionAction) self.posture_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_grasp_action') rospy.loginfo('waiting for wubble_grasp_status service') rospy.wait_for_service('/wubble_grasp_status') self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus) rospy.loginfo('connected to wubble_grasp_status service') rospy.loginfo('waiting for classify service') rospy.wait_for_service('/classify') self.classification_srv = rospy.ServiceProxy('/classify', classify) rospy.loginfo('connected to classify service') # connect to gripper action server rospy.loginfo('waiting for wubble_gripper_action') self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction) self.gripper_controller.wait_for_server() rospy.loginfo('connected to wubble_gripper_action') # connect to wubble actions rospy.loginfo('waiting for drop_object') self.drop_object_client = SimpleActionClient('/drop_object', DropObjectAction) self.drop_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for grasp_object') self.grasp_object_client = SimpleActionClient('/grasp_object', GraspObjectAction) self.grasp_object_client.wait_for_server() rospy.loginfo('connected to grasp_object') rospy.loginfo('waiting for lift_object') self.lift_object_client = SimpleActionClient('/lift_object', LiftObjectAction) self.lift_object_client.wait_for_server() rospy.loginfo('connected to lift_object') rospy.loginfo('waiting for place_object') self.place_object_client = SimpleActionClient('/place_object', PlaceObjectAction) self.place_object_client.wait_for_server() rospy.loginfo('connected to plcae_object') rospy.loginfo('waiting for push_object') self.push_object_client = SimpleActionClient('/push_object', PushObjectAction) self.push_object_client.wait_for_server() rospy.loginfo('connected to push_object') rospy.loginfo('waiting for shake_roll_object') self.shake_roll_object_client = SimpleActionClient( '/shake_roll_object', ShakeRollObjectAction) self.shake_roll_object_client.wait_for_server() rospy.loginfo('connected to drop_object') rospy.loginfo('waiting for shake_pitch_object') self.shake_pitch_object_client = SimpleActionClient( '/shake_pitch_object', ShakePitchObjectAction) self.shake_pitch_object_client.wait_for_server() rospy.loginfo('connected to pitch_object') rospy.loginfo('waiting for ready_arm') self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction) self.ready_arm_client.wait_for_server() rospy.loginfo('connected to ready_arm') # advertise InfoMax service rospy.Service('get_category_distribution', InfoMax, self.process_infomax_request) rospy.loginfo( 'all services contacted, object_categorization is ready to go') self.ACTION_INFO = { InfomaxAction.GRASP: { 'client': self.grasp_object_client, 'goal': GraspObjectGoal(), 'prereqs': [InfomaxAction.GRASP] }, InfomaxAction.LIFT: { 'client': self.lift_object_client, 'goal': LiftObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT] }, InfomaxAction.SHAKE_ROLL: { 'client': self.shake_roll_object_client, 'goal': ShakeRollObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_ROLL ] }, InfomaxAction.DROP: { 'client': self.drop_object_client, 'goal': DropObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP] }, InfomaxAction.PLACE: { 'client': self.place_object_client, 'goal': PlaceObjectGoal(), 'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE] }, InfomaxAction.PUSH: { 'client': self.push_object_client, 'goal': PushObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE, InfomaxAction.PUSH ] }, InfomaxAction.SHAKE_PITCH: { 'client': self.shake_pitch_object_client, 'goal': ShakePitchObjectGoal(), 'prereqs': [ InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_PITCH ] }, }