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')
