def start_perception(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartPerception')
rospy.loginfo('Subscribing to /suturo/perception_node_status.')
self.__subscriber = rospy.Subscriber('/suturo/perception_node_status',
PerceptionNodeStatus,
self.wait_for_perception)
rospy.sleep(1)
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_4:
task_num = '4'
elif task_type == Task.TASK_6:
task_num = '6'
else:
task_num = '1'
global perception_process
perception_process, perception_logger_process =\
utils.start_node('roslaunch euroc_launch perception_task' + task_num + '.launch',
resp.taskdata.initialization_time,
resp.taskdata.logging, 'Perception')
while not self.__perception_ready:
time.sleep(1)
rospy.loginfo('Sucessfully started Perception')
resp.result = 'success'
return resp
def start_perception(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartPerception')
rospy.loginfo('Subscribing to /suturo/perception_node_status.')
self.__subscriber = rospy.Subscriber('/suturo/perception_node_status', PerceptionNodeStatus,
self.wait_for_perception)
rospy.sleep(1)
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_4:
task_num = '4'
elif task_type == Task.TASK_6:
task_num = '6'
else:
task_num = '1'
global perception_process
perception_process, perception_logger_process =\
utils.start_node('roslaunch euroc_launch perception_task' + task_num + '.launch',
resp.taskdata.initialization_time,
resp.taskdata.logging, 'Perception')
while not self.__perception_ready:
time.sleep(1)
rospy.loginfo('Sucessfully started Perception')
resp.result = 'success'
return resp
def start_manipulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing TestNode init.')
subprocess.Popen('rosrun euroc_launch TestNode --init', shell=True)
rospy.loginfo('Executing state StartManipulation')
rospy.loginfo('Subscribing to /suturo/manipulation_node_status.')
self.__nodes_subscriber = rospy.Subscriber('/suturo/manipulation_node_status', ManipulationNodeStatus,
self.wait_for_manipulation)
rospy.loginfo('Subscribign to /move_group/status topic.')
rospy.sleep(1)
self.__move_group_status_subscriber = rospy.Subscriber('/move_group/status', GoalStatusArray,
self.wait_for_move_group_status)
global manipulation_process
manipulation_process, manipulation_logger_process =\
utils.start_node('roslaunch euroc_launch manipulation.launch', resp.taskdata.initialization_time,
resp.taskdata.logging, 'Manipulation')
while not self.__move_group_status and not self.__manipulation_nodes_ready:
time.sleep(1)
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_6:
global manipulation_conveyor_frames_process
rospy.loginfo('Starting publish_conveyor_frames.')
manipulation_conveyor_frames_process, manipulation_conveyor_frames_logger_process =\
utils.start_node('rosrun suturo_planning_manipulation publish_conveyor_frames.py',
resp.taskdata.initialization_time, resp.taskdata.logging, 'Conveyor frames')
manipulation_conveyor_frames_process = manipulation_conveyor_frames_process
else:
manipulation_conveyor_frames_process = None
rospy.loginfo('Sucessfully started Manipulation')
resp.result = 'success'
return resp
def _handle_focus_object(self, req):
# Read the objects_to_focus if this is the first iteration
taskdata = req.taskdata
taskdata.objects_to_focus = taskdata.classified_objects
if self._objects_to_focus is None:
self._objects_to_focus = taskdata.objects_to_focus
print("Objects to focus1: ")
print(self._objects_to_focus)
if not taskdata.focused_point is None:
object_to_focus = None
min_dist = 100
for obj in self._objects_to_focus:
dist = mathemagie.euclidean_distance(
obj.c_centroid, taskdata.focused_point)
if dist < min_dist:
min_dist = dist
object_to_focus = obj
self._objects_to_focus = [object_to_focus]
# Remember the fitted objects
elif not taskdata.fitted_object is None:
self._fitted_objects.append(taskdata.fitted_object)
if not utils.map is None:
position = taskdata.fitted_object.mpe_object.primitive_poses[
0].position
utils.map.mark_region_as_object_under_point(
position.x, position.y)
rospy.logdebug(str(utils.map))
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
# If there are no more objects to focus
if not self._objects_to_focus:
self._objects_to_focus = None
print("Objects to focus is None 2")
taskdata.fitted_objects = self._fitted_objects
self._fitted_objects = []
return TaskDataServiceResponse(taskdata=taskdata, result="success")
# Set the next object to focus
next_object = self._objects_to_focus.pop()
print("Objects to focus2")
print(self._objects_to_focus)
taskdata.object_to_focus = next_object
print("nect_object:")
print(next_object)
if utils.is_handle(next_object.object.id, taskdata.yaml):
return TaskDataServiceResponse(taskdata=taskdata,
result="focusHandle")
else:
return TaskDataServiceResponse(taskdata=taskdata,
result="focusObject")
def start_classifier(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartClassifier')
global classifier_process
classifier_process, classifier_logger_process =\
utils.start_node('rosrun suturo_perception_classifier classifier.py', req.taskdata.initialization_time,
req.taskdata.logging, 'Classifier')
resp.result = 'success'
rospy.loginfo('Sucessfully started Classifier')
return resp
def start_classifier(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartClassifier')
global classifier_process
classifier_process, classifier_logger_process =\
utils.start_node('rosrun suturo_perception_classifier classifier.py', req.taskdata.initialization_time,
req.taskdata.logging, 'Classifier')
resp.result = 'success'
rospy.loginfo('Sucessfully started Classifier')
return resp
def stop_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopSimulation')
if not executed_test_node_check:
check_node(resp.taskdata.initialization_time, resp.taskdata.logging)
if self.savelog:
save_task()
stop_task()
time.sleep(3)
rospy.loginfo('Finished state StopSimulation')
resp.result = 'success'
return resp
def stop_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopSimulation')
if not executed_test_node_check:
check_node(resp.taskdata.initialization_time,
resp.taskdata.logging)
if self.savelog:
save_task()
stop_task()
time.sleep(3)
rospy.loginfo('Finished state StopSimulation')
resp.result = 'success'
return resp
def start_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartSimulation')
resp.taskdata.yaml = suturo_planning_task_selector.start_task(resp.taskdata.name)
rospy.loginfo('Got YAML description')
resp.taskdata.objects_found = []
rospy.loginfo('Waiting for clock.')
self.clock = rospy.Subscriber('clock', Clock, self.wait_for_clock)
while not self.new_clock:
time.sleep(1)
rospy.loginfo('Sucessfully started Simulation')
resp.result = 'success'
return resp
def start_simulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StartSimulation')
resp.taskdata.yaml = suturo_planning_task_selector.start_task(
resp.taskdata.name)
rospy.loginfo('Got YAML description')
resp.taskdata.objects_found = []
rospy.loginfo('Waiting for clock.')
self.clock = rospy.Subscriber('clock', Clock, self.wait_for_clock)
while not self.new_clock:
time.sleep(1)
rospy.loginfo('Sucessfully started Simulation')
resp.result = 'success'
return resp
def _handle_scan_map(self, req):
taskdata = req.taskdata
rospy.loginfo('Scanning map')
utils.map = Map(2)
self._scan_map()
return TaskDataServiceResponse(taskdata=taskdata,
result=self.RETURN_VAL_MAP_SCANNED)
def _handle_focus(self, req):
taskdata = req.taskdata
rospy.loginfo('Executing state FocusObject')
rospy.loginfo('Trying to focus %s' %
taskdata.object_to_focus.object.id)
taskdata.focused_object = None
centroid = taskdata.object_to_focus.c_centroid
centroid.z -= 0.03
poses = calc_grasp_position.make_scan_pose(
centroid, *utils.focus_poses[self._ctr])
utils.manipulation.set_planning_time_arm(2)
move_method = utils.manipulation.move_to
transition = ""
for pose in poses:
if move_method(pose):
taskdata.focused_object = taskdata.object_to_focus
rospy.logdebug('Wait for clock')
time.sleep(1)
rospy.logdebug('Wait for tf again.')
rospy.sleep(4)
self._ctr = 0
transition = self.RETURN_VAL_SUCSS
if transition == "":
utils.manipulation.set_planning_time_arm(5)
if self._ctr < 6:
self._ctr += 1
transition = self.RETURN_VAL_RETRY
else:
self._ctr = 0
transition = self.RETURN_VAL_FAIL
return TaskDataServiceResponse(taskdata=taskdata, result=transition)
def stop_nodes(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopNodes.')
if perception_process is not None:
exterminate(perception_process.pid, signal.SIGKILL, r=True)
if manipulation_process is not None:
exterminate(manipulation_process.pid, signal.SIGKILL, r=True)
if manipulation_conveyor_frames_process is not None:
exterminate(manipulation_conveyor_frames_process.pid, signal.SIGKILL, r=True)
if classifier_process is not None:
exterminate(classifier_process.pid, signal.SIGKILL, r=True)
rospy.signal_shutdown('Finished plan. Shutting down Node.')
time.sleep(3)
rospy.loginfo('Finished state StopNodes.')
resp.result = 'success'
return resp
def stop_nodes(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state StopNodes.')
if perception_process is not None:
exterminate(perception_process.pid, signal.SIGKILL, r=True)
if manipulation_process is not None:
exterminate(manipulation_process.pid, signal.SIGKILL, r=True)
if manipulation_conveyor_frames_process is not None:
exterminate(manipulation_conveyor_frames_process.pid,
signal.SIGKILL,
r=True)
if classifier_process is not None:
exterminate(classifier_process.pid, signal.SIGKILL, r=True)
rospy.signal_shutdown('Finished plan. Shutting down Node.')
time.sleep(3)
rospy.loginfo('Finished state StopNodes.')
resp.result = 'success'
return resp
def handle_search_objects(self, req):
"""
Servicehandler for the service NAME_SERVICE
:param taskdata: The given Servicerequest - Parameter
:return: The required response object
"""
rospy.loginfo("Searching objects")
taskdata = req.taskdata
self._check_missing_objects(taskdata.yaml.objects)
self._check_found_objects(taskdata.fitted_objects, taskdata.objects_found)
self._print_found_objects()
state_transition = self._check_all_objects_found()
return TaskDataServiceResponse(taskdata = taskdata, result = state_transition)
def _handle_refocus(self, taskdata):
rospy.loginfo('Executing state FocusHandle')
filter_handle = [
obj for obj in taskdata.classified_objects
if obj.object.id == taskdata.focused_object.object.id
]
if filter_handle:
taskdata.object_to_focus = filter_handle[0]
transition = 'focusHandle'
else:
transition = 'noHandle'
return TaskDataServiceResponse(taskdata=taskdata, result=transition)
def determine_task_type(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state DetermineTaskType')
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_1:
ret = 'task1'
elif task_type == Task.TASK_2:
ret = 'task2'
elif task_type == Task.TASK_3:
ret = 'task3'
elif task_type == Task.TASK_4:
ret = 'task4'
elif task_type == Task.TASK_5:
ret = 'task5'
elif task_type == Task.TASK_6:
ret = 'task6'
else:
ret = 'fail'
resp.result = ret
rospy.loginfo('Executing task is from type ' + str(ret) + '.')
return resp
def determine_task_type(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing state DetermineTaskType')
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_1:
ret = 'task1'
elif task_type == Task.TASK_2:
ret = 'task2'
elif task_type == Task.TASK_3:
ret = 'task3'
elif task_type == Task.TASK_4:
ret = 'task4'
elif task_type == Task.TASK_5:
ret = 'task5'
elif task_type == Task.TASK_6:
ret = 'task6'
else:
ret = 'fail'
resp.result = ret
rospy.loginfo('Executing task is from type ' + str(ret) + '.')
return resp
def start_manipulation(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
rospy.loginfo('Executing TestNode init.')
subprocess.Popen('rosrun euroc_launch TestNode --init', shell=True)
rospy.loginfo('Executing state StartManipulation')
rospy.loginfo('Subscribing to /suturo/manipulation_node_status.')
self.__nodes_subscriber = rospy.Subscriber(
'/suturo/manipulation_node_status', ManipulationNodeStatus,
self.wait_for_manipulation)
rospy.loginfo('Subscribign to /move_group/status topic.')
rospy.sleep(1)
self.__move_group_status_subscriber = rospy.Subscriber(
'/move_group/status', GoalStatusArray,
self.wait_for_move_group_status)
global manipulation_process
manipulation_process, manipulation_logger_process =\
utils.start_node('roslaunch euroc_launch manipulation.launch', resp.taskdata.initialization_time,
resp.taskdata.logging, 'Manipulation')
while not self.__move_group_status and not self.__manipulation_nodes_ready:
time.sleep(1)
task_type = resp.taskdata.yaml.task_type
if task_type == Task.TASK_6:
global manipulation_conveyor_frames_process
rospy.loginfo('Starting publish_conveyor_frames.')
manipulation_conveyor_frames_process, manipulation_conveyor_frames_logger_process =\
utils.start_node('rosrun suturo_planning_manipulation publish_conveyor_frames.py',
resp.taskdata.initialization_time, resp.taskdata.logging, 'Conveyor frames')
manipulation_conveyor_frames_process = manipulation_conveyor_frames_process
else:
manipulation_conveyor_frames_process = None
rospy.loginfo('Sucessfully started Manipulation')
resp.result = 'success'
return resp
def _handle_scan_shadow(self, req):
taskdata = req.taskdata
rospy.loginfo('Executing state ScanShadow')
scan_poses = ['shadow_pose1', 'shadow_pose2']
for scan_pose in scan_poses:
rospy.loginfo('Taking pose: %s' % scan_pose)
utils.manipulation.move_to(scan_pose)
rospy.sleep(utils.waiting_time_before_scan)
# Add pc to map
rospy.logdebug('Adding current point cloud to map')
arm_base = utils.manipulation.get_base_origin()
utils.map.add_point_cloud(arm_base.point, scene_cam=False)
# Creating planning scene from map
rospy.logdebug('Creating planning scene from map')
utils.map.all_unknowns_to_obstacle()
rospy.logdebug(str(utils.map))
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
return TaskDataServiceResponse(taskdata = taskdata, result = self.RET_VAL_DONE)
def __handle_call(self, req):
response = TaskDataServiceResponse()
taskdata = req.taskdata
response.result = self.__scan_obstacles(taskdata)
response.taskdata = taskdata
return response
def init(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
self.create_manipulation()
resp.result = 'success'
return resp
def __handle_request(self, request):
taskdata = request.taskdata
result = self.execute(taskdata)
return TaskDataServiceResponse(taskdata=taskdata, result=result)
def __handle_request(self, request):
taskdata = request.taskdata
#taskdata.clean_up_plan = map(lambda action: (action.object, action), taskdata.clean_up_plan)
result = self.execute(taskdata)
return TaskDataServiceResponse(taskdata=taskdata, result=result)
def _handle_pose_estimate_object(self, req):
rospy.loginfo('Executing state PoseEstimateObject')
taskdata = req.taskdata
taskdata.focused_object = taskdata.object_to_focus
taskdata.sec_try = False
taskdata.fitted_object = None
# Get the ID of the classified object from the YAML file
ids = utils.get_yaml_objects_nrs(taskdata.yaml,
taskdata.focused_object.object.id)
# pose_estimated = perception.get_gripper_perception(pose_estimation=True, object_ids=ids)[0] # OLD
pose_estimated_objects = perception.get_gripper_perception(
pose_estimation=True, cuboid=False, object_ids=ids)
rospy.logdebug("Returned pose_estimated_objects:")
rospy.logdebug(str(pose_estimated_objects))
if pose_estimated_objects is None:
rospy.logwarn('Couldn\'t get gripper perception.')
if not taskdata.sec_try_done:
taskdata.sec_try = True
return TaskDataServiceResponse(taskdata=taskdata, result="fail")
if not pose_estimated_objects:
rospy.logdebug('No object found for pose estimation.')
if not taskdata.sec_try_done:
taskdata.sec_try = True
return TaskDataServiceResponse(taskdata=taskdata, result="fail")
# Convert the poses to odom_combined
map(lambda obj: utils.euroc_object_to_odom_combined(obj),
pose_estimated_objects)
# TODO Investigate the result for the object that's closest to the original object we were interested in
# TODO find proper threshold
corresponding_object_idx = utils.get_nearest_object_idx(
taskdata.focused_object, pose_estimated_objects, 0.1)
# # TODO if more than one object were recognized, classify again and take the closest
# if len(pose_estimated_objects) == 1:
# corresponding_object_idx = 0
# else:
# corresponding_object_idx = None
# for idx in range(0, len(pose_estimated_objects)):
# if pose_estimated_objects[idx].mpe_success:
# corresponding_object_idx = idx
# TODO Call the perception again
if corresponding_object_idx is None:
rospy.logdebug(
'Couldn\'t find the desired object on the next lookup again')
if not taskdata.sec_try_done:
taskdata.sec_try = True
return TaskDataServiceResponse(taskdata=taskdata, result="fail")
pose_estimated = pose_estimated_objects[corresponding_object_idx]
rospy.logdebug('Use Result from Object :%s' % str(pose_estimated))
if pose_estimated is None or not pose_estimated.mpe_success:
rospy.logwarn('Poseestimation failed.')
if not taskdata.sec_try_done:
taskdata.sec_try = True
return TaskDataServiceResponse(taskdata=taskdata, result="fail")
taskdata.fitted_object = pose_estimated
rospy.loginfo('Publishing objects into planning scene.')
utils.publish_collision_objects([pose_estimated.mpe_object])
return TaskDataServiceResponse(taskdata=taskdata, result="success")
def __handle_request(self, req):
taskdata = req.taskdata
result = self.__grasp(taskdata)
return TaskDataServiceResponse(taskdata = taskdata, result = result)
def __handle_call(self, req):
response = TaskDataServiceResponse()
taskdata = req.taskdata
response.result = self.__scan_obstacles(taskdata)
response.taskdata = taskdata
return response
def init(self, req):
resp = TaskDataServiceResponse()
resp.taskdata = req.taskdata
self.create_manipulation()
resp.result = 'success'
return resp
def __handle_call(self, req):
resp = TaskDataServiceResponse()
resp.result = self.__classify(req.taskdata)
resp.taskdata = req.taskdata
return resp
