def start_aubo_feeding(): # 创建客户端 client = actionlib.ActionClient("/aubo/ctrl", ArmWorkAction) # 等待连接服务器 client.wait_for_server() # 发送请求 goal = ArmWorkGoal() goal.type = 0 goal_handle = client.send_goal(goal) result = wait_for_result(goal_handle, rospy.Duration(30)) if result: terminal_state = goal_handle.get_terminal_state() get_result = goal_handle.get_result() return terminal_state == TerminalState.SUCCEEDED, get_result return False, None
def __init__(self): # Initialize Action Client self._ac = actionlib.ActionClient('/action_iot_ros', msgIotRosAction) # Dictionary to Store all the goal handels self._goal_handles = {} # Store the MQTT Topic on which to Publish in a variable param_config_iot = rospy.get_param('config_iot') self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] # Wait for Action Server that will use the action - '/action_iot_ros' to start self._ac.wait_for_server() rospy.loginfo("Action server up, we can send goals.")
def __init__(self): """ The Constructor for CameraActionClient. It sets up the bridge between openCV and ROS. It will also, setup this node as an publisher to "/eyrc/vb/camera_1/image_raw" ROS topic. """ self.bridge = CvBridge() # Subscribing to ROS "/eyrc/vb/camera_1/image_raw" topic to get an input image self.image_sub = rospy.Subscriber("/eyrc/vb/camera_1/image_raw", Image, self.callback) # Creating a handle to publish the messages to a topic self.pub_handle = rospy.Publisher('/pkg_task5/package_info', String, queue_size=10) # Dictionary to store, the package name and color as a key value pair self.package_pose_color = { "packagen00": "NA", "packagen01": "NA", "packagen02": "NA", "packagen10": "NA", "packagen11": "NA", "packagen12": "NA", "packagen20": "NA", "packagen21": "NA", "packagen22": "NA", "packagen30": "NA", "packagen31": "NA", "packagen32": "NA" } self.all_info_aquired = False self.packages_updated_on_gsheet = 0 # Making it action client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) # Dictionary to store the goal handles self._goal_handles = {} # Wait for the action server to start rospy.loginfo("[CAMERA CLIENT] Waiting for Action server ...") self._ac.wait_for_server()
def __init__(self): # Initialize Action Client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) # list to Store all the goal handels self._goal_handles = [] # Store the MQTT Topic on which to Publish in a variable param_config_pyiot = rospy.get_param('config_pyiot') self._config_mqtt_pub_topic = param_config_pyiot['mqtt']['topic_pub'] self._config_spread_sheet_id = param_config_pyiot['google_apps'][ 'spread_sheet_id'] self._config_my_spread_sheet_id = "AKfycbzh5VbH9ZYzlebU6DCewMO3qq25OoGGEgvt_2nRbR0gtE5Cp5K0" # Wait for Action Server that will use the action - '/action_ros_iot' to start self._ac.wait_for_server() rospy.loginfo("Action server ROS Iot Bridge is up, we can send goals.")
def start_slam_blanding(): # 创建客户端 client = actionlib.ActionClient("/slam/move", ArmWorkAction) # 等待连接服务器 client.wait_for_server() # 发送请求 goal = CarMoveGoal() goal.type = 1 goal_handle = client.send_goal(goal) result = wait_for_result(goal_handle, rospy.Duration(60)) if result: terminal_state = goal_handle.get_terminal_state() get_result = goal_handle.get_result() return terminal_state == TerminalState.SUCCEEDED, get_result return False, None
def _connectToServers(self): rospy.loginfo('Waiting for "' + self.ASSERV_GOTO_SERVICE_NAME + '"...') rospy.wait_for_service(self.ASSERV_GOTO_SERVICE_NAME) rospy.loginfo('Asserv found.') # Goto service try: self._asservGotoService = rospy.ServiceProxy( self.ASSERV_GOTO_SERVICE_NAME, Goto) self._asservGotoActionClient = actionlib.ActionClient( self.ASSERV_GOTOACTION_NAME, DoGotoAction) self._asservGotoActionClient.wait_for_server() except rospy.ServiceException, e: error_str = "Error when trying to connect to " error_str += self.ASSERV_GOTO_SERVICE_NAME error_str += " : " + str(e) rospy.logfatal(error_str)
def __init__(self, name, catalog, warehouse_init_state, stacker_node, liability_node): self._server = actionlib.ActionServer('supplier', TransportAction, self.plan_job, auto_start=False) self._server.start() self._jobs_proc_thread = threading.Thread(target=self._jobs_proc) self._jobs_proc_thread.daemon = True self._jobs_proc_thread.start() self.warehouse = dict() # warehouses order service proxy for content in catalog: # call: resp = warehouse.get(content)() -> {ok, q} self.warehouse.update({ content : rospy.ServiceProxy('/warehouse/raws/' + content + '/order', WarehouseOrder) }) if warehouse_init_state == 'full': rospy.wait_for_service('/warehouse/raws/' + content + '/fill_all') fill_srv = rospy.ServiceProxy('/warehouse/raws/' + content + '/fill_all', WarehouseFillAll) fill_srv() elif warehouse_init_state == 'empty': rospy.wait_for_service('/warehouse/raws/' + content + '/empty_all') empty_srv = rospy.ServiceProxy('/warehouse/raws/' + content + '/empty_all', WarehouseEmptyAll) empty_srv() self.stacker = actionlib.ActionClient(stacker_node, StackerAction) self.stacker.wait_for_server() rospy.wait_for_service(liability_node +'/finish') self.finish = rospy.ServiceProxy(liability_node + '/finish', Empty) self.current_gh = None def step(request): rospy.loginfo('Supplier running step %d...', request.step) self.make_bids() return StepResponse() rospy.Service('~step', Step, step) rospy.wait_for_service('/market/gen_bids') self.bid = rospy.ServiceProxy('/market/gen_bids', BidsGenerator) subscnt = SubscribersCounter() self.signing_bid = rospy.Publisher('/market/signing/bid', Bid, subscriber_listener=subscnt, queue_size=128) while subscnt.num_peers < 1: # wait for market node rospy.sleep(0.3) self.make_bids() rospy.logdebug('Supplier node initialized')
def start_laser_mark(name, type, id): # 创建客户端 client = actionlib.ActionClient("/laser/mark", LaserMarkAction) # 等待连接服务器 client.wait_for_server() # 发送请求 goal = LaserMarkGoal() goal.name = name goal.type = type goal.id = id goal_handle = client.send_goal(goal) result = wait_for_result(goal_handle, rospy.Duration(30)) if result: terminal_state = goal_handle.get_terminal_state() get_result = goal_handle.get_result() return terminal_state == TerminalState.SUCCEEDED, get_result return False, None
def __init__(self, action_name="performance_test"): """ Creates an L{actionlib.ActionClient} object and waits for connection to the action server. @raise Exception: if the connection does not succeed @type action_name: str @param action_name: action server name, by default this is C{"performance_test"}, but if the action server lives in a different namespace than the full path should be given (e.g. C{"/source2/performance_test"}) """ self.actionclient = actionlib.ActionClient(action_name, msgs.LinktestAction) self.count = 0 started = self.actionclient.wait_for_server( ) # wait until the udpmonsourcenode has started up if started is False: raise Exception, "could not connect to action server"
def __init__(self): self.ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) self._goal_handles = {} param_config_iot = rospy.get_param('config_iot') self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_sub_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self.parameter = {} self.ac.wait_for_server() rospy.loginfo("Action server up") self._handle_ros_sub = rospy.Subscriber(self._config_mqtt_sub_topic, msgMqttSub, self.callback) self.sac = actionlib.SimpleActionClient('/action_turtle', msgTurtleAction) self.sac.wait_for_server() rospy.loginfo("Action client up , to send goal")
def __init__(self): # load params for this robot/client into dict self.client_params = rospy.get_param('/client_params') print(self.client_params) print('Action Server Init') # Active/Primary Task server self.feedback_sub = rospy.Subscriber( '~active_feedback', String, self.update_active_feedback) self._action_name = name self._as = actionlib.SimpleActionServer( "~active", TaskAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() # Continous Task tracking/server self.running_continuous_tasks = {} self.continuous_lock = threading.RLock() self._as_continuous = actionlib.ActionServer( "~continuous", TaskAction, self.start_continuous_task, self.stop_continuous_task, auto_start=False) self._as_continuous.start() print('connecting to own action interfaces...') self.continuous_client = actionlib.ActionClient( '~/continuous', TaskAction) self.continuous_client.wait_for_server() # find the workspace so we can get tasks later current_path = os.path.abspath(__file__) pkg_name = rospkg.get_package_name(current_path) ws_name = current_path.split('src/')[0] self.ws_name = ws_name[:-1]
def _connectToServers(self): rospy.loginfo('Waiting for "' + self.ASSERV_GOTO_SERVICE_NAME + '"...') rospy.wait_for_service(self.ASSERV_GOTO_SERVICE_NAME) rospy.loginfo("Asserv found.") # Goto service try: self._asservGotoService = rospy.ServiceProxy( self.ASSERV_GOTO_SERVICE_NAME, Goto) self._asservGotoActionClient = actionlib.ActionClient( self.ASSERV_GOTOACTION_NAME, DoGotoAction) self._asservGotoActionClient.wait_for_server() self._asservManageService = rospy.ServiceProxy( self.ASSERV_MANAGE_SERVICE_NAME, Management) self._asservEmergencyStopService = rospy.ServiceProxy( self.ASSERV_EMERSTP_SERVICE_NAME, EmergencyStop) except rospy.ServiceException as e: error_str = "Error when trying to connect to " error_str += self.ASSERV_GOTO_SERVICE_NAME error_str += " : " + str(e) rospy.logfatal(error_str)
def __init__(self): # Initialize Action Client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) # list to Store all the goal handels self._goal_handles = [] # Store the MQTT Topic on which to Publish in a variable param_config_pyiot = rospy.get_param('config_pyiot') self._config_mqtt_pub_topic = param_config_pyiot['mqtt']['topic_pub'] self._config_spread_sheet_id = param_config_pyiot['google_apps'][ 'spread_sheet_id'] self._config_submission_spread_sheet_id = param_config_pyiot[ 'google_apps']['submission_spread_sheet_id'] # Wait for Action Server that will use the action - '/action_ros_iot' to start self._ac.wait_for_server() rospy.loginfo("Action server ROS Iot Bridge is up, we can send goals.") rospy.Subscriber('/ros_iot_bridge/mqtt/sub', msgMqttSub, self.orders_callback) self._orders_to_dispatch = [] self._flag_lock = 0
def round_robin_server(cX, cY, tX, tY, steps, repetitions): # Create the clients clients = [] for address in node_addresses: clients.append( actionlib.ActionClient('route_planner_' + address, offload.msg.RoutePlanAction)) # Wait for the servers for client in clients: client.wait_for_server() # Create the goal goal = offload.msg.RoutePlanGoal(cX=cX, cY=cY, tX=tX, tY=tY, steps=steps) # Create list to store the ClientActionHandlers handlers = [] sendStart = time() # Send the requests in round robin fashion for i in range(0, repetitions): handlers.append(clients[i % len(clients)].send_goal(goal)) # wait for the goal from the same client to be received while i >= len(clients) - 1 and handlers[ i + 1 - len(clients)].get_goal_status() == GoalStatus.PENDING: rospy.sleep(0.1) continue sendEnd = time() rospy.loginfo("Sent all goals in %d seconds", sendEnd - sendStart) # Wait until each handler return successfully for i in range(0, repetitions): while handlers[i].get_goal_status() != GoalStatus.SUCCEEDED: #rospy.loginfo("handlers[%d].status == %s", i, handlers[i].get_goal_status()) rospy.sleep(0.2) # Prints out the result of executing the action return handlers[0].get_result() # A FibonacciResult
def __init__(self): self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/eyrc/vb/camera_1/image_raw", Image, self.camera1_callback) ## MQTT Client # Initialize Action Client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) param_config_iot = rospy.get_param('config_pyiot') # Store the ROS Topic to get the start message from bridge action server self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] # Dictionary to Store all the goal handels self._goal_handles = {} # Wait for Action Server that will use the action - '/action_iot_ros' to start self._ac.wait_for_server() rospy.loginfo("Action server up, we can send goals.") self.image_captured_flag = False
def fibonacci_client_single_server(n, repetitions): # Creates the ActionClient, passing the type of the action # (FibonacciAction) to the constructor. client = actionlib.ActionClient('fib_server_' + socket.gethostname(), offload.msg.FibonacciAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = offload.msg.FibonacciGoal(order=n) # Create list to store the ClientActionHandlers handlers = [] # Send the specified number of requests for i in range(0, repetitions): handlers.append(client.send_goal(goal)) # wait for the goal to be received while handlers[i].get_goal_status() == GoalStatus.PENDING: rospy.sleep(0.2) continue rospy.loginfo("handlers len: %d", len(handlers)) # Wait until each handler return successfully for i in range(0, repetitions): #status = handlers[i].get_goal_status() #rospy.loginfo("handlers[%d].status == %s", i, status) while handlers[i].get_goal_status() != GoalStatus.SUCCEEDED: rospy.sleep(0.2) #status = handlers[i].get_goal_status() # rospy.loginfo("handlers[%d].status == %s", i, status) # Prints out the result of executing the action return handlers[0].get_result() # A FibonacciResult
def __init__(self): rospy.init_node('node_pick_place_iot_client') self._ac1 = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) self._ac2 = actionlib.SimpleActionClient('/action_pick', msgPickPlaceAction) self._ac3 = actionlib.SimpleActionServer('/action_place', msgPickPlaceAction, execute_cb=self.on_goal, auto_start=False) self._goal_handles1 = {} self._goal_handles2 = {} self._goal_handles3 = {} self._goal_info = {} self.count1 = 0 self.count2 = 0 self.count3 = 0 self._ac1.wait_for_server() rospy.loginfo('Action Iot server up ready....') self._ac2.wait_for_server() rospy.loginfo('Pick server up ready....') self.colour = {'Medicine': 'red', 'Food': 'yellow', 'Clothes': 'green'} self.item = {'red': 'Medicine', 'yellow': 'Food', 'green': 'Clothes'} self.priority = {'red': 'HP', 'yellow': 'MP', 'green': 'LP'} self.cost = {'red': 450, 'yellow': 250, 'green': 150} self.processing_goal_place = '' self._processing_goal = '' self._processing_goal_handle = '' self.largest_goal_handle = '' self.largest_goal = '' rospy.loginfo('Place server up ready....') self._sub_mqtt_sub = rospy.Subscriber('/ros_iot_bridge/mqtt/sub', msgMqttSub, self.func_callback_msgMqttSub) goal = msgPickPlaceGoal() goal.message = 'None' self._processing_goal_handle = self._ac2.send_goal( goal, done_cb=self.done_callback) self._processing_goal = goal.message self.count2 = self.count2 + 1 self._goal_info.update({self.count2: goal.message}) self._ac3.start()
def __init__(self): self.ac = actionlib.ActionClient(self.TOPIC, mbdm.ArcMoveAction) self.goal = None self.gh = None self._lock = threading.RLock() return
def __init__(self, arg_robot_name): '''CLASS INITIALIZATION INITIALIZE ALL VARIABLES.''' rospy.init_node('node_t5_ur5_1', anonymous=True) self._robot_ns = '/' + arg_robot_name self._planning_group = "manipulator" param_config_iot = rospy.get_param('config_iot') self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_sub_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._commander = moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander( robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._scene = moveit_commander.PlanningSceneInterface( ns=self._robot_ns) self._group = moveit_commander.MoveGroupCommander( self._planning_group, robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self.ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) self._activate_vacuum_gripper = rospy.ServiceProxy( '/eyrc/vb/ur5/activate_vacuum_gripper/ur5_1', vacuumGripper) self._activate_ConveyorBelt = rospy.ServiceProxy( '/eyrc/vb/conveyor/set_power', conveyorBeltPowerMsg) self.conveyor_state = rospy.Subscriber('/gazebo_sim/conveyor/state', ConveyorBeltState, self.conveyor_state_callback) self.color = rospy.Publisher('/topic_package_Color', packageColor, queue_size=20) self.logical_camera_1 = rospy.Subscriber("/eyrc/vb/logical_camera_1", LogicalCameraImage, self.callback_logical_cam_1, queue_size=1) self.orders = rospy.Subscriber(self._config_mqtt_sub_topic, msgMqttSub, self.callback_orders, queue_size=10) self.ac.wait_for_server() self._exectute_trajectory_client.wait_for_server() self._planning_frame = self._group.get_planning_frame() self._eef_link = self._group.get_end_effector_link() self._group_names = self._robot.get_group_names() self._box_name = '' self.bridge = CvBridge() self._goal_handles = {} self.red_pkgs = [] self.yellow_pkgs = [] self.green_pkgs = [] self.orders_list = [] self.packagen_00 = [ math.radians(162), math.radians(-115), math.radians(8), math.radians(-90), math.radians(19), math.radians(0) ] self.packagen_01 = [ math.radians(130), math.radians(-89), math.radians(-29), math.radians(-69), math.radians(46), math.radians(0) ] self.packagen_02 = [ math.radians(57), math.radians(-109), math.radians(-7), math.radians(-67), math.radians(119), math.radians(0) ] self.packagen_10 = [ math.radians(-52), math.radians(-95), math.radians(83), math.radians(-171), math.radians(-128), math.radians(0) ] self.packagen_11 = [ math.radians(125), math.radians(-74), math.radians(-64), math.radians(136), math.radians(-57), math.radians(0) ] self.packagen_12 = [ math.radians(60), math.radians(-91), math.radians(-49), math.radians(143), math.radians(-105), math.radians(0) ] self.packagen_20 = [ math.radians(-51), math.radians(-97), math.radians(95), math.radians(1), math.radians(128), math.radians(0) ] self.packagen_21 = [ math.radians(122), math.radians(-60), math.radians(-105), math.radians(167), math.radians(-59), math.radians(0) ] self.packagen_22 = [ math.radians(53), math.radians(-82), math.radians(-92), math.radians(176), math.radians(-127), math.radians(0) ] self.packagen_30 = [ math.radians(-51), math.radians(-89), math.radians(116), math.radians(-30), math.radians(129), math.radians(0) ] self.packagen_31 = [ math.radians(-117), math.radians(-118), math.radians(135), math.radians(-19), math.radians(62), math.radians(0) ] self.packagen_32 = [ math.radians(-163), math.radians(-97), math.radians(122), math.radians(-30), math.radians(17), math.radians(0) ] self.package_drop = [ math.radians(173), math.radians(-42), math.radians(56), math.radians(-104), math.radians(-90), math.radians(-7) ] self.straightUp = [ math.radians(180), math.radians(-90), math.radians(0), math.radians(-180), math.radians(-90), math.radians(0) ] # Attribute to store computed trajectory by the planner self._computed_plan = '' # Current State of the Robot is needed to add box to planning scene self._curr_state = self._robot.get_current_state() rospy.loginfo('\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m') rospy.loginfo('\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m') rospy.loginfo('\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m') rp = rospkg.RosPack() self._pkg_path = rp.get_path('pkg_task5') self._file_path = self._pkg_path + '/config/saved_trajectories/' rospy.loginfo("Package Path: {}".format(self._file_path)) rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m')
def __init__(self, context): super(ExperimentGUI, self).__init__(context) # Give QObjects reasonable names self.plans = [ 'Starting Position', 'Above Panel', 'Panel Grabbed', 'Above Placement Nest', 'Panel Placed' ] #state_dict ties each state to planlistindex values #self.state_dict={'reset_position':0,'pickup_prepare':1,'pickup_lower':2,'pickup_grab_first_step':2,'pickup_grab_second_step':2,'pickup_raise':2,'transport_panel':3,'place_lower':4,'place_set_first_step':4,'place_set_second_step':4,'place_raise':4} self.gui_execute_states = [ "reset", "panel_pickup", "pickup_grab", "transport_panel", "place_panel" ] self.execute_states = [ ['plan_to_reset_position', 'move_to_reset_position'], ['plan_pickup_prepare', 'move_pickup_prepare'], [ 'plan_pickup_lower', 'move_pickup_lower', 'plan_pickup_grab_first_step', 'move_pickup_grab_first_step', 'plan_pickup_grab_second_step', 'move_pickup_grab_second_step', 'plan_pickup_raise', 'move_pickup_raise' ], ['plan_transport_payload', 'move_transport_payload'], ['plan_place_set_second_step'] ] self.teleop_modes = [ 'Error', 'Off', 'Joint', 'Cartesian', 'Cylindrical', 'Spherical' ] self.current_teleop_mode = 1 self.teleop_button_string = "Tele-op\nMode:\n" self.setObjectName('MyPlugin') self._lock = threading.Lock() #self._send_event=threading.Event() self.controller_commander = controller_commander_pkg.ControllerCommander( ) # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) if not args.quiet: print 'arguments: ', args print 'unknowns: ', unknowns # Create QWidget self.in_process = None self.recover_from_pause = False #rospy.get_param("rosbag_name") #<param name="start_time" command="date +'%d-%m-%Y_%Ih%Mm%S'"/> #rosbag record args="record -O arg('start_time') self._mainwidget = QWidget() self.layout = QGridLayout() self._mainwidget.setLayout(self.layout) self.disconnectreturnoption = False self.stackedWidget = QStackedWidget() #self._mainwidget) self.layout.addWidget(self.stackedWidget, 0, 0) self._welcomescreen = QWidget() self._runscreen = QWidget() self._errordiagnosticscreen = QWidget() self.stackedWidget.addWidget(self._welcomescreen) self.stackedWidget.addWidget(self._runscreen) self.stackedWidget.addWidget(self._errordiagnosticscreen) #self._data_array=collections.deque(maxlen=500) self._proxy_model = message_proxy_model.MessageProxyModel() self._rospack = rospkg.RosPack() #self.console=console_widget.ConsoleWidget(self._proxy_model,self._rospack) self.robotconnectionled = LEDIndicator() self.robotconnectionled.setDisabled(True) # Make the led non clickable self.forcetorqueled = LEDIndicator() self.forcetorqueled.setDisabled(True) # Make the led non clickable self.overheadcameraled = LEDIndicator() self.overheadcameraled.setDisabled(True) # Make the led non clickable self.grippercameraled = LEDIndicator() self.grippercameraled.setDisabled(True) # Make the led non clickable self.runscreenstatusled = LEDIndicator() self.runscreenstatusled.setDisabled(True) self.step_executor = GUI_Step_Executor() self.step_executor.error_signal.connect(self._feedback_receive) #self.step_executor.error_function=self._feedback_receive #Need this to pause motions self.process_client = actionlib.ActionClient('process_step', ProcessStepAction) self.process_client.wait_for_server() self.placement_targets = { 'leeward_mid_panel': 'panel_nest_leeward_mid_panel_target', 'leeward_tip_panel': 'panel_nest_leeward_tip_panel_target' } self.placement_target = 'panel_nest_leeward_mid_panel_target' self.panel_type = 'leeward_mid_panel' self.client_handle = None service_list = rosservice.get_service_list() if ('/overhead_camera/trigger' in service_list): self.led_change(self.overheadcameraled, True) else: self.led_change(self.overheadcameraled, False) if ('/gripper_camera_2/trigger' in service_list): self.led_change(self.grippercameraled, True) else: self.led_change(self.grippercameraled, False) self.mode = 0 #self.rewound=False self.count = 0 self.data_count = 0 self.force_torque_data = np.zeros((6, 1)) self.joint_angle_data = np.zeros((6, 1)) # Get path to UI file which should be in the "resource" folder of this package self.welcomescreenui = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'resource', 'welcomeconnectionscreen.ui') self.runscreenui = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'resource', 'Runscreen.ui') self.errorscreenui = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'resource', 'errordiagnosticscreen.ui') self.rewound = False # Extend the widget with all attributes and children from UI file loadUi(self.welcomescreenui, self._welcomescreen) loadUi(self.runscreenui, self._runscreen) loadUi(self.errorscreenui, self._errordiagnosticscreen) # Give QObjects reasonable names self._mainwidget.setObjectName('MyPluginUi') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: self._mainwidget.setWindowTitle(self._mainwidget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._mainwidget) self.context = context self.plugin_settings = None self.instance_settings = None #self._errordiagnosticscreen.consoleWidget=console_widget.ConsoleWidget(self._proxy_model,self._rospack) #####consoleiagnosticscreen.backToRun.pressed.connect(self._to_run_screen) #self._runscThread=ConsoleThread(self._widget.State_info) # self._welcomescreen.statusFormLayout.takeAt(0) self._welcomescreen.statusFormLayout.addWidget(self.robotconnectionled, 0, 0) self._welcomescreen.statusFormLayout.addWidget(self.forcetorqueled, 2, 0) self._welcomescreen.statusFormLayout.addWidget(self.overheadcameraled, 4, 0) self._welcomescreen.statusFormLayout.addWidget(self.grippercameraled, 6, 0) self._runscreen.connectionLayout.addWidget(self.runscreenstatusled, 0, 1) #self._welcomescreen.robotConnectionWidget.addWidget(self.led) #consoleThread.finished.connect(app.exit) #####consoleThread.start() self.rviz_starter = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'src', 'rpi_arm_composites_manufacturing_gui', 'rviz_starter.py') # Add widget to the user interface #context.add_widget(console)==QDialog.Accepted #context.add_widget(rqt_console) for entry in self.plans: listentry = QListWidgetItem(entry) listentry.setFlags(Qt.ItemIsSelectable) self._runscreen.planList.addItem(listentry) self._runscreen.planList.item(0).setSelected(True) self.planListIndex = 0 self._runscreen.panelType.setText(self.panel_type) self._runscreen.placementNestTarget.setText("Leeward Mid Panel Nest") self._runscreen.panelType.setReadOnly(True) self._runscreen.placementNestTarget.setReadOnly(True) self.commands_sent = False rospy.Subscriber("controller_state", controllerstate, self.callback) self._set_controller_mode = rospy.ServiceProxy("set_controller_mode", SetControllerMode) rospy.Subscriber("GUI_state", ProcessState, self.process_state_set) #rospy.Subscriber('gui_error', String, self._feedback_receive()) self.force_torque_plot_widget = QWidget() self.joint_angle_plot_widget = QWidget() self._welcomescreen.openConfig.clicked.connect( self._open_config_options) #self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_login_prompt) self._welcomescreen.toRunScreen.pressed.connect(self._to_run_screen) self._runscreen.backToWelcome.pressed.connect(self._to_welcome_screen) #self._runscreen.toErrorScreen.pressed.connect(self._to_error_screen) self._runscreen.nextPlan.pressed.connect(self._next_plan) self._runscreen.previousPlan.pressed.connect(self._previousPlan) self._runscreen.resetToHome.pressed.connect(self._reset_position) self._runscreen.stopPlan.pressed.connect(self._stopPlan) self._runscreen.accessTeleop.pressed.connect(self.change_teleop_modes) #self._errordiagnosticscreen.openOverheadCameraView.pressed.connect(self._open_overhead_camera_view) #self._errordiagnosticscreen.openGripperCameraViews.pressed.connect(self._open_gripper_camera_views) self._errordiagnosticscreen.openForceTorqueDataPlot.pressed.connect( self._open_force_torque_data_plot) self._errordiagnosticscreen.openJointAngleDataPlot.pressed.connect( self._open_joint_angle_data_plot) self._errordiagnosticscreen.backToRun.pressed.connect( self._to_run_screen)
def __init__(self): self._original_orders = [] self.HigherPriorityOrder = [] self.ur5_1_home_pose = [ math.radians(-90), math.radians(-90), math.radians(0), math.radians(-90), math.radians(-90), math.radians(90) ] self.ur5_1_conveyor_pose = [ math.radians(7.8), math.radians(-139.4), math.radians(-57.6), math.radians(-72.8), math.radians(89.9), math.radians(7.8) ] ur5_1_pkg00 = [ math.radians(-55.8), math.radians(-67.0), math.radians(1.2), math.radians(-114.1), math.radians(-121.3), math.radians(90) ] ur5_1_pkg01 = [ math.radians(-118.9), math.radians(-85.6), math.radians(18.7), math.radians(-113.1), math.radians(-61.0), math.radians(90.0) ] ur5_1_pkg02 = [ math.radians(55.7), math.radians(-117.0), math.radians(5.4), math.radians(-68.4), math.radians(124.2), math.radians(90) ] ur5_1_pkg10 = [ math.radians(-55.1), math.radians(-96.9), math.radians(82.6), math.radians(-165.7), math.radians(-124.8), math.radians(90) ] ur5_1_pkg11 = [ math.radians(-122.7), math.radians(-116.5), math.radians(95.9), math.radians(-159.3), math.radians(-57.2), math.radians(90.0) ] ur5_1_pkg12 = [ math.radians(54.4), math.radians(-84.5), math.radians(-83.6), math.radians(-9.3), math.radians(126.7), math.radians(90) ] ur5_1_pkg20 = [ math.radians(-55.09), math.radians(-96.44), math.radians(87.31), math.radians(9.035), math.radians(125.49), math.radians(90) ] ur5_1_pkg21 = [ math.radians(116.01), math.radians(-61.96), math.radians(-129.27), math.radians(10.33), math.radians(62.64), math.radians(90) ] ur5_1_pkg22 = [ math.radians(55.5), math.radians(-85.8), math.radians(-114.2), math.radians(20.8), math.radians(124.7), math.radians(90.0) ] ur5_1_pkg30 = [ math.radians(-55.08), math.radians(-91.64), math.radians(117.76), math.radians(-26.22), math.radians(125.48), math.radians(90.0) ] ur5_1_pkg31 = [ math.radians(-121.6), math.radians(-115.9), math.radians(135.1), math.radians(-19.2), math.radians(58.3), math.radians(90) ] ur5_1_pkg32 = [ math.radians(-160.73), math.radians(-92.61), math.radians(118.27), math.radians(-25.89), math.radians(19.84), math.radians(90) ] # Names of packages and their respective bins in gazebo self.packages_name_position = { "packagen00": ur5_1_pkg00, "packagen01": ur5_1_pkg01, "packagen02": ur5_1_pkg02, "packagen10": ur5_1_pkg10, "packagen11": ur5_1_pkg11, "packagen12": ur5_1_pkg12, "packagen20": ur5_1_pkg20, "packagen21": ur5_1_pkg21, "packagen22": ur5_1_pkg22, "packagen30": ur5_1_pkg30, "packagen31": ur5_1_pkg31, "packagen32": ur5_1_pkg32 } # Initialize ROS Node rospy.init_node('node_t5_ur5_1_package_pick', anonymous=True) rospy.sleep(15) self.publish_orders = rospy.Publisher('/Orders_to_ship', String, queue_size=10) # Wait for service rospy.wait_for_service('/2Dcamera_packages_type') # Load variables for moveit! self._robot_ns = '/' + "ur5_1" self._planning_group = "manipulator" self._commander = moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander( robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._scene = moveit_commander.PlanningSceneInterface( ns=self._robot_ns) self._group = moveit_commander.MoveGroupCommander( self._planning_group, robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=2) self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self._exectute_trajectory_client.wait_for_server() rospy.set_param('/ur5_1_vacuum_gripper_service', False) self._planning_frame = self._group.get_planning_frame() self._eef_link = self._group.get_end_effector_link() self._group_names = self._robot.get_group_names() self._computed_plan = '' self._curr_state = self._robot.get_current_state() self._group.set_planning_time(99) rp = rospkg.RosPack() self._pkg_path = rp.get_path('pkg_task5') self._file_path = self._pkg_path + '/config/saved_trajectories/' rospy.loginfo("Package Path: {}".format(self._file_path)) rospy.loginfo('\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m') rospy.loginfo('\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m') rospy.loginfo('\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m') rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m') ## MQTT Client # Initialize Action Client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) param_config_iot = rospy.get_param('config_pyiot') # Store the ROS Topic to get the start message from bridge action server self._param_order_topic = param_config_iot['mqtt']['sub_cb_ros_topic'] self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] # Subscribe to the desired topic and attach a Callback Funtion to it. rospy.Subscriber(self._param_order_topic, msgMqttSub, self.func_callback_orders) # Dictionary to Store all the goal handels self._goal_handles = {} self._orders = [] self._package_colours = None # Wait for Action Server that will use the action - '/action_iot_ros' to start self._ac.wait_for_server() rospy.loginfo("Action server up, we can send goals.")
def __init__(self): self.al_client = actionlib.ActionClient("tf_lookup", TLM.TfLookupAction) self.cbs = {} self.ts = {} self.ghs = []
def __init__(self, name, catalog, warehouse_init_state, stacker_node, liability_node): self._server = actionlib.ActionServer('storage', TransportAction, self.plan_job, auto_start=False) self._server.start() rospy.logdebug('Storage.ActionServer ready') rospy.wait_for_service(liability_node + '/finish') self.finish = rospy.ServiceProxy(liability_node + '/finish', Empty) rospy.logdebug('Storage.finish ready') self.warehouse = dict() # warehouses place service proxy for content in catalog: rospy.logdebug('Storage.wh.%s starting' % content) rospy.wait_for_service('/warehouse/goods/' + content + '/place') self.warehouse.update({ content: rospy.ServiceProxy('/warehouse/goods/' + content + '/place', WarehousePlace) }) rospy.logdebug('Storage.wh.%s place' % content) if warehouse_init_state == 'full': rospy.wait_for_service('/warehouse/goods/' + content + '/fill_all') rospy.logdebug('Storage.wh.%s.full' % content) fill_srv = rospy.ServiceProxy( '/warehouse/goods/' + content + '/fill_all', WarehouseFillAll) fill_srv() elif warehouse_init_state == 'empty': rospy.wait_for_service('/warehouse/goods/' + content + '/empty_all') rospy.logdebug('Storage.wh.%s.empty' % content) empty_srv = rospy.ServiceProxy( '/warehouse/goods/' + content + '/empty_all', WarehouseEmptyAll) empty_srv() rospy.logdebug('Storage.wh.%s ready' % content) rospy.logdebug('Storage.warehouse ready') rospy.wait_for_service('/transport_goods/conveyor/conveyor/load') self.conveyor_load = rospy.ServiceProxy( '/transport_goods/conveyor/conveyor/load', ConveyorLoad) rospy.wait_for_service( '/transport_goods/conveyor/conveyor/destination') self.conveyor_destination = rospy.ServiceProxy( '/transport_goods/conveyor/conveyor/destination', ConveyorDestination) # 'warehouse' or 'garbage' rospy.wait_for_service( '/transport_goods/conveyor/conveyor/product_ready') self.conveyor_product_ready = rospy.ServiceProxy( '/transport_goods/conveyor/conveyor/product_ready', ConveyorProductReady) # 'warehouse' or 'garbage' rospy.logdebug('Goods conveyor proxy ready') self.stacker = actionlib.ActionClient(stacker_node, StackerAction) self.stacker.wait_for_server() rospy.logdebug('Stacker client ready') self.current_gh = None rospy.logdebug('Creating job_proc_thread') self._jobs_proc_thread = threading.Thread(target=self._jobs_proc) self._jobs_proc_thread.daemon = True self._jobs_proc_thread.start() def step(request): rospy.loginfo('Storage running step %d...', request.step) self.make_bids() return StepResponse() rospy.Service('~step', Step, step) rospy.wait_for_service('/market/gen_bids') self.bid = rospy.ServiceProxy('/market/gen_bids', BidsGenerator) subscnt = SubscribersCounter() self.signing_bid = rospy.Publisher('/market/signing/bid', Bid, subscriber_listener=subscnt, queue_size=128) while subscnt.num_peers < 1: # wait for market node rospy.sleep(0.3) self.make_bids() rospy.logdebug('Storage node initialized')
def __init__(self, arg_robot_name): rospy.init_node('node_t5_ur5_2', anonymous=True) self._robot_ns = '/' + arg_robot_name self._planning_group = "manipulator" param_config_iot = rospy.get_param('config_iot') self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_sub_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._commander = moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander( robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._scene = moveit_commander.PlanningSceneInterface( ns=self._robot_ns) self._group = moveit_commander.MoveGroupCommander( self._planning_group, robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self.ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) self._exectute_trajectory_client.wait_for_server() self.ac.wait_for_server() self._activate_vacuum_gripper = rospy.ServiceProxy( '/eyrc/vb/ur5/activate_vacuum_gripper/ur5_2', vacuumGripper) self._activate_ConveyorBelt = rospy.ServiceProxy( '/eyrc/vb/conveyor/set_power', conveyorBeltPowerMsg) self.conveyor_state = rospy.Subscriber('/gazebo_sim/conveyor/state', ConveyorBeltState, self.conveyor_state_callback) self._planning_frame = self._group.get_planning_frame() self._eef_link = self._group.get_end_effector_link() self._group_names = self._robot.get_group_names() self._box_name = '' # Attribute to store computed trajectory by the planner self._computed_plan = '' # Current State of the Robot is needed to add box to planning scene self._curr_state = self._robot.get_current_state() rospy.loginfo('\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m') rospy.loginfo('\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m') rospy.loginfo('\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m') rp = rospkg.RosPack() self._pkg_path = rp.get_path('pkg_task5') self._file_path = self._pkg_path + '/config/saved_trajectories/' rospy.loginfo("Package Path: {}".format(self._file_path)) rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m') self.red_bin = [ math.radians(69), math.radians(-41), math.radians(80), math.radians(-131), math.radians(-91), math.radians(-1) ] self.yellow_bin = [ math.radians(-3), math.radians(-41), math.radians(80), math.radians(-131), math.radians(-91), math.radians(-1) ] self.green_bin = [ math.radians(-93), math.radians(-41), math.radians(80), math.radians(-131), math.radians(-91), math.radians(-1) ] self.home_pose = [ math.radians(172.158644502), math.radians(-40.0489379456), math.radians(58.2512942738), math.radians(-108.158242078), math.radians(-90.0037747571), math.radians(-7.86462864917) ] self.package_Color = rospy.Subscriber("/topic_package_Color", packageColor, self.callback_color) self.color_list = []
#!/usr/bin/env python import sys import rospy import actionlib from face_recognition.msg import FaceRecognitionAction, FaceRecognitionGoal if __name__ == '__main__': rospy.init_node('face_recognition_starter') c = actionlib.ActionClient('face_recognition', FaceRecognitionAction) c.wait_for_server() g = FaceRecognitionGoal() g.order_id = 1 g.order_argument = "none" c.send_goal(g) sys.exit(0)
def __init__(self): # self.pose_sub = rospy.Subscriber("/laser_amcl_basic_pose", PoseStamped, self.update_pose) self.current_pose = None self.goal = None self.goalHandle = None self.actionClient = actionlib.ActionClient("/navigate", NavigateAction)
def __init__(self, arg_robot_name): """ Constructor This intiates the robot and required arguments in this class. """ rospy.init_node('node_ur5_2', anonymous=True) self._ac_ros_iot = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) self._robot_ns = '/' + arg_robot_name self._planning_group = "manipulator" moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander( robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns ) self._scene = moveit_commander.PlanningSceneInterface(ns=self._robot_ns) self._group = moveit_commander.MoveGroupCommander( self._planning_group, robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns ) self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1 ) self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction ) self._exectute_trajectory_client.wait_for_server() self._planning_frame = self._group.get_planning_frame() self._eef_link = self._group.get_end_effector_link() self._group_names = self._robot.get_group_names() self.presence_of_package = False # Current State of the Robot is needed to add box to planning scene self._curr_state = self._robot.get_current_state() rospy.loginfo( '\033[94m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m') rospy.loginfo( '\033[94m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m') rospy.loginfo( '\033[94m' + "Group Names: {}".format(self._group_names) + '\033[0m') relative_path = rospkg.RosPack() self._pkg_path = relative_path.get_path('pkg_moveit_examples') self._file_path = self._pkg_path + '/config/' rospy.loginfo("Package Path: {}".format(self._file_path)) self.pkg_detect_flag = False self.pkgname_w = [] self.color_w = [] self.city_w = [] self.orderid_w = [] param_config_iot = rospy.get_param('config_iot') self._config_shipped_pub_topic = param_config_iot['http']['shipped'] self._goal_handles = {} self._ac_ros_iot.wait_for_server() rospy.loginfo("Action server is up, we can send new goals!") rospy.loginfo('\033[94m' + " >>> Ur5Moveit init done." + '\033[0m')
def __init__(self): self._ac = actionlib.ActionClient('/count_until', CountUntilAction) self._ac.wait_for_server() rospy.loginfo("Action server up, we can send goals.") self._goal_handles = {}
#! /usr/bin/env python import roslib roslib.load_manifest('rosoclingo_examples') from rosoclingo.msg import * import rospy import actionlib client = actionlib.ActionClient('ROSoClingo', ROSoClingoAction) def send_goal(content): goal = ROSoClingoGoal("moveto(" + str(content) + ")") print "send: " + "bring(" + str(content) + ")" return client.send_goal(goal) def example(): handlers = [] client.wait_for_server() goal = "6" handlers.append(send_goal(goal)) rospy.sleep(10.0) goal = "3" handlers.append(send_goal(goal)) rospy.sleep(40.0) goal = "2"
def __init__(self): """ The constructor for Ur5MoveitActionClient. This function will initalize this node as ROS Action Client. Moveit configurations for UR5_2 will also be setup here """ # Configuring Moveit self._robot_ns = '/' + 'ur5_2' self._planning_group = "manipulator" self._commander = moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander( robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._scene = moveit_commander.PlanningSceneInterface( ns=self._robot_ns) self._group = moveit_commander.MoveGroupCommander( self._planning_group, robot_description=self._robot_ns + "/robot_description", ns=self._robot_ns) self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self._exectute_trajectory_client.wait_for_server() self._planning_frame = self._group.get_planning_frame() self._eef_link = self._group.get_end_effector_link() self._group_names = self._robot.get_group_names() # Subscribe to "/eyrc/vb/onConveyor" to get the info on dispatched packages rospy.Subscriber("/eyrc/vb/onConveyor", onConveyor, self.cb_onConveyor) # A dict to store all the processed info on color of pkgs self.package_pose_color = {} # A list to store all the shipped models self.shipped_pkgs = [] # A dict to store all the dispatched models self.dispatched_pkg = {} # Attribute to store computed trajectory by the planner self._computed_plan = '' # Set to True if Currently Executing a Path self._exec_curr_path = False # Current State of the Robot is needed to add box to planning scene self._curr_state = self._robot.get_current_state() # Setting up the path of the saved trajectories rp = rospkg.RosPack() self._pkg_path = rp.get_path('pkg_task5') self._file_path = self._pkg_path + '/config/saved_trajectories/' rospy.loginfo("Package Path: {}".format(self._file_path)) # Making it Action Client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) # Dictionary to store the goal handles self._goal_handles = {} # Wait for the action server to start rospy.loginfo("[UR5_2] Waiting for Action Server ...") self._ac.wait_for_server() rospy.loginfo('\033[94m' + "[UR5_2]: >>> Ur5Moveit init done." + '\033[0m')