def wrapper(*args, **kwargs):
response = TriggerResponse()
try:
f(*args, **kwargs)
response.success = True
except expected_exception_types as e:
rospy.logerr(e.message)
response.success = False
response.message = e.message
return response
def train_operator_callback(self, req):
print 'traing operator'
result = TriggerResponse()
result.success = False
with self._l:
if self._rgb_img is None:
return result
if self.memorize_operator(self._rgb_img):
result.success = True
return result
result.success = False
return result
def left_interaction_get_ready_cb(self, req):
resp = TriggerResponse()
if self.left_arm_mann:
rospy.logerr('Left arm in interactive mode')
resp.success = False
else:
resp.success = self.move(self.group_left, target="up_left_arm")
return resp
def right_interaction_arm_up_cb(self, req):
resp = TriggerResponse()
if self.right_arm_mann:
rospy.logerr('Right arm in interactive mode')
resp.success = False
else:
resp.success = self.move(self.group_right, target="up_right_arm")
return resp
def zero_offset(self):
resp = TriggerResponse()
self._flag_zero_offset = True
time.sleep(30)
self._flag_zero_offset = False
if len(self._offset_data) >= 2500:
IMU_Fix.zero_offset = sum(self._offset_data) / len(self._offset_data)
resp.success = True
resp.message = '零偏初始化成功'
else:
resp.success = False
resp.message = '零偏初始化失败'
def stop(self, req):
res = TriggerResponse()
try:
self.trigger = False
res.success = True
except (rospy.ServiceException, rospy.ROSException) as e:
res.success = False
print("Service call failed: %s" % e)
return res
def emergency_land_service(request):
global emergency_land_called, armed
responce = TriggerResponse()
if armed:
responce.success = True
responce.message = "Start emergency landing"
emergency_land_called = True
else:
responce.success = False
responce.message = "Copter is disarmed, no need for emergency landing!"
emergency_land_called = False
return responce
def train_operator_callback(self, req):
print 'traing operator'
result = TriggerResponse()
result.success = False
with self._l:
if self._rgb_img is None:
return result
if self.memorize_operator(self._rgb_img):
result.success = True
return result
result.success = False
return result
def listen_hdl(self, req):
response = TriggerResponse()
r, msg = self.recognize_sphinx()
if r:
response.success = r
response.message = msg
s = String()
s.data = msg
self.result_pub.publish(s)
else:
response.success = r
response.message = msg
return response
def right_interaction_off_cb(self, req):
resp = TriggerResponse()
if not self.right_arm_mann:
rospy.logerr('Right arm already in normal mode')
resp.success = False
else:
rospy.loginfo('Right arm interactive mode OFF')
resp.success = True
self.right_arm_mann = False
self.left_int_pub.publish(False)
return resp
def program_resume_cb(self, req):
resp = TriggerResponse()
resp.success = True
if not self.executing_program:
resp.success = False
resp.message = "Program si not running!"
elif not self.program_paused:
resp.success = False
resp.message = "Program is not paused"
else:
self.program_paused = False
rospy.Timer(rospy.Duration(1),
self.program_resume_timer_cb,
oneshot=True)
return resp
def handleStartService(self, request):
print('[Service {}] Starting Detection'.format(self.name_))
response = TriggerResponse()
self.mutex_.acquire()
if self.is_running_:
self.mutex_.release()
response.success = False
return response
self.mutex_.release()
self.is_running_ = True
Thread(target=self.talker).start()
response.success = True
return response
def save_calibration(req):
global sensor
response = TriggerResponse()
try:
calibration = np.array(sensor.get_calibration())
np.save(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'calibration'), calibration)
response.success = True
response.message = str(calibration)
except Exception as e:
response.success = False
response.message = str(e)
return response
return response
def relative_position_move(request):
global tony_info
global target_pose1
if (tony_info == 0):
#print('Cannot find target')
trigger_result = TriggerResponse()
trigger_result.success = False
trigger_result.message = 'cannot receive any target message'
else:
print('zt')
moveitDemo = MoveItIkDemo()
moveitDemo.move_the_arm(target_pose1)
trigger_result = TriggerResponse()
trigger_result.success = True
return trigger_result
def look_at_me_cb(self, req):
self.look_at_me(False, 0.3)
self.task1 = False
resp = TriggerResponse()
resp.success = True
return resp
def go_home(request):
#print('zt')
moveitDemo = MoveItIkDemo()
moveitDemo.move_to_home()
trigger_result = TriggerResponse()
trigger_result.success = True
return trigger_result
def handle_reset(self, msg):
self.sum = 0
self.n = 0
result = TriggerResponse()
result.success = True
return result
def my_callback(request):
turt = moveTurtlebot()
my_response = TriggerResponse()
las = Laser_topic()
count = 5
rospy.sleep(1)
print(las.laser_info())
x, y, z = las.laser_info()
while x > 1.4:
turt.move_speed(1, 0.5)
turt.move_turtle('forwards')
x, y, z = las.laser_info()
print(x, y, z)
rospy.sleep(0.5)
turt.move_speed(0, 0)
turt.move_turtle('stop')
x, y, z = las.laser_info()
print(x, y, z)
rospy.sleep(1)
if y > 1.5:
my_response.message = 'Its about to hit obstacle, Move right'
else:
my_response.message = 'Its about to hit obstacle, Move left'
rospy.sleep(1)
my_response.success = True
return my_response
def del_one_model(self):
if len(self.added_models) > 0:
delReq = DeleteModelRequest()
delReq.model_name = self.added_models.pop()
tmp = self.modelsPoses.pop()
tmpResp = self.delete_model_proxy(delReq)
rospy.loginfo("delete model to gazebo success: " +
str(tmpResp.success))
if tmpResp.success:
info = "Deleted model: " + str(delReq.model_name) + " succeed!"
rospy.loginfo(info)
else:
status_message = "Delete failed due to unknown reason"
rospy.logwarn(status_message)
tmpResp.status_message = status_message
else:
status_message = "No model to be deleted"
rospy.logwarn(status_message)
tmpResp = DeleteModelResponse()
tmpResp.status_message = status_message
resp = TriggerResponse()
resp.success = tmpResp.success
resp.message = tmpResp.status_message
return resp
def detect_stop_service(request):
global detected_objects, last_image_area
responce = TriggerResponse()
if detected_objects['stop']:
x, y, w, h = get_frame(detected_objects['stop'])
object_area = w * h
if object_area > dyn_params.stop_area * last_image_area:
responce.success = True
responce.message = "Stop sign detected!"
else:
responce.success = False
responce.message = "Stop sign is detected, but small!"
else:
responce.success = False
responce.message = "Stop sign is not detected!"
return responce
def __command(self, req):
"""
ROS service callback, handle command requests and route them to the Gazebo plugin.
:param req The SimulationRecorder request, see definition for details.
:return SimulationRecorderResponse with success of command and any status/error message.
"""
# call the appropriate service based on the request type
if req.request_type == srv.SimulationRecorderRequest.STATE:
resp = self.__recorder_state()
elif req.request_type == srv.SimulationRecorderRequest.START:
resp = self.__recorder_start()
elif req.request_type == srv.SimulationRecorderRequest.STOP:
resp = self.__recorder_stop()
elif req.request_type == srv.SimulationRecorderRequest.CANCEL:
resp = self.__recorder_cancel()
# reset the recorder by discarding any saved files
elif req.request_type == srv.SimulationRecorderRequest.RESET:
resp = self.__recorder_cleanup()
# invalid request type, notify caller of failure
else:
resp = TriggerResponse()
resp.success = False
resp.message = "Invalid Simulation Recorder command: %s" % str(req.request_type)
# populate our internal response based on the actual call
return srv.SimulationRecorderResponse(value=resp.success, message=resp.message)
def handle_move_arm_to_waypoint(self, request):
# create a response object for the trigger service
response = TriggerResponse()
# check if the trajectory finished
trajectory_finished = self.trajectory_index >= self.trajectory.shape[0]
# if trajectory isn't finished
if not trajectory_finished:
# get the latest joint values from given trajectory
joint_values = trajectory[self.trajectory_index, :]
# create command, i.e., a dictionary of joint names and values
command = dict(zip(self.joint_names, joint_values))
# move the limb to given joint angle
try:
self.limb.move_to_joint_positions(command)
response.message = 'Successfully moved arm to the following waypoint %s' % command
except rospy.exceptions.ROSException:
response.message = 'Error while moving arm to the following waypoint %s' % command
finally:
# increment the counter
self.trajectory_index += 1
else:
response.message = 'Arm trajectory is finished already'
# set the success parameter of the response object
response.success = trajectory_finished
# set the flag just before returning from the function so that it is
# almost certain that the service request is returned successfully
self.trajectory_finished = trajectory_finished
return response
def service_callback(self, request):
response = TriggerResponse()
response.success = True
response.message = "x: " + str(self.topicData.pose.pose.position.x) + \
" ,y: " + str(self.topicData.pose.pose.position.y) + \
" ,z: " + str(self.topicData.pose.pose.position.z)
return response
def handle_add_two_ints(request):
#global target_pose1
to_move_x = -1 * target_pose1.position.y
to_move_y = -1 * target_pose1.position.x
to_move_z = -1 * target_pose1.position.z + 0.09
print(to_move_x)
print(to_move_y)
print(to_move_z)
moveitDemo = MoveItIkDemo()
gripper_client = actionlib.SimpleActionClient('/gripper_move',
GripperMoveAction)
gripper_move(gripper_client, 0.03, 10, 10, 10)
moveitDemo.translate(1, to_move_y)
moveitDemo.translate(0, to_move_x)
#moveitDemo.translate(1,to_move_y)
moveitDemo.translate(2, to_move_z)
rospy.sleep(5)
moveitDemo.translate(1, 0.038)
moveitDemo.translate(0, 0.06)
moveitDemo.translate(2, -0.04)
# gripper_client = actionlib.SimpleActionClient('/gripper_move', GripperMoveAction)
gripper_move(gripper_client, 0.0115, 10, 10, 10)
moveitDemo.arm.set_named_target('home')
moveitDemo.arm.go()
trigger_result = TriggerResponse()
trigger_result.success = True
return trigger_result
def cb_save(self, req):
res = TriggerResponse()
srv_name = self.save_srv.resolved_name
rospy.logdebug('Called service: {}'.format(srv_name))
num_samples = self.get_num_samples()
if num_samples > 0:
# Save poses as rosbag
path = self.path
bag = rosbag.Bag(os.path.expanduser(path), 'w')
try:
stamp = rospy.Time.now()
for ee_pose, marker_pose in zip(self.ee_samples,
self.marker_samples):
bag.write('ee_poses', ee_pose, stamp)
bag.write('marker_poses', marker_pose, stamp)
stamp += rospy.Duration(
1.0) # Add one second between poses
res.success = True
msg = '{0} pose(s) have been written to: {1}'.format(
num_samples, path)
except:
msg = 'Failed to write poses to: {0}'.format(path)
finally:
bag.close()
else:
msg = 'Please capture poses before saving'
rospy.loginfo(msg)
res.message = msg
return res
def reset_initial(msg):
global first
response = TriggerResponse()
first = True
response.success = True
rospy.loginfo("Initial value reseted")
return response
def restart_controller(req):
"""Callback function to restart the gimbal controller. This function will
terminate the ROS node if it encounters a SerialException.
Arg:
req: Request object for the ROS Service.
Returns: Service Response, "True" for success, "False" for error.
"""
try:
response = TriggerResponse()
success = gimbal.restart_controller()
response.success = success
if success:
response.message = "Gimbal restarted successfully!"
# Shutdown the node on success restart
sub.unregister()
pub_timer.shutdown()
camera_pub.unregister()
controller_pub.unregister()
rospy.Timer(rospy.Duration(0.1), restart_shutdown_callback, True)
else:
response.message = "Gimbal restart failed!"
return response
except serial.serialutil.SerialException as e:
rospy.logfatal(e)
rospy.signal_shutdown("SerialError: {0:s}".format(e))
def add_one_model(self):
req = self.__getReq()
if req is not None:
tmpResp = self.spawn_model_proxy(req)
else:
tmpResp = SpawnModelResponse()
tmpResp.success = False
tmpResp.status_message = "No model in Bin"
if tmpResp.success:
rospy.loginfo("add model to gazebo success: " +
str(tmpResp.success))
info = "Add model: '" + str(self.req.model_name), "' at : " + str(
self.req.initial_pose) + "(xyz,quaternion) succeed!"
rospy.loginfo(info)
# ipdb.set_trace()
self.added_models.append(deepcopy(self.req.model_name))
self.modelsPoses.append(deepcopy(self.req.initial_pose))
self.noisyPoses.append(deepcopy(self.req.initial_pose))
else:
status_message = "Add model failed"
rospy.logwarn(status_message)
tmpResp.status_message = status_message + ", " + tmpResp.status_message
resp = TriggerResponse()
resp.success = tmpResp.success
resp.message = tmpResp.status_message
return resp
def program_stop_cb(self, req):
resp = TriggerResponse()
resp.success = True
if self.executing_program:
rospy.logdebug('Stopping program')
self.executing_program = False
if self.program_paused:
self.program_paused = False
rospy.Timer(rospy.Duration(0, 100),
self.program_resume_timer_cb,
oneshot=True)
else:
resp.success = False
resp.message = "Program is not running"
return resp
def handleIsEnabledSrv(self, req):
try:
res = TriggerResponse()
res.success = self.backgroundMovementProxy.isEnabled()
return res
except RuntimeError, e:
rospy.logerr("Exception caught:\n%s", e)
return None
def right_interaction_move_to_user_cb(self, req):
resp = TriggerResponse()
if self.right_arm_mann:
rospy.logerr('Right arm in interactive mode')
resp.success = False
else:
pose = PoseStamped()
pose.pose.position.x = 0.7
pose.pose.position.y = -0.1
pose.pose.position.z = 1.15
pose.pose.orientation.w = 1
pose.header.frame_id = "base_link"
resp.success = self.move(self.group_right, pose=pose)
return resp
def _get_resp(self, request):
#rospy.loginfo("Server received request")
self._detection_dict, side_dir_recommended = self._laser_scan_reader.get_detection()
response = TriggerResponse()
response.success = self._crash_detected()
response.message = str(self._get_dir_message(side_dir_recommended))
return response
def handleStopService(self, request):
response = TriggerResponse()
print('[Service {}] Stopping Detection'.format(self.name_))
self.mutex_.acquire()
self.is_running_ = False
self.mutex_.release()
response.success = True
return response
def _save(self, req):
rospy.logerr('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
rospy.logerr('Save request is called to image buffer.')
rospy.logerr('<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<')
self.stamp = rospy.Time.now()
self.pub_imgs = None
res = TriggerResponse()
res.success = True
return res
def left_interaction_move_to_user_cb(self, req):
resp = TriggerResponse()
if self.left_arm_mann:
rospy.logerr('Left arm in interactive mode')
resp.success = False
else:
pose = PoseStamped()
pose.pose.position.x = 0.7
pose.pose.position.y = 0.1
pose.pose.position.z = 1.2
pose.pose.orientation.w = 1
pose.header.frame_id = "base_link"
# pose_transformed = self.tf_listener.transformPose(pose, self.group_left.get_planning_frame())
resp.success = self.move(self.group_left, pose=pose)
return resp
def handleGetLifeSrv(self, req):
try:
res = TriggerResponse()
res.success = True
res.message = self.lifeProxy.getState()
rospy.loginfo("current life state is " + str(res.message))
return res
except RuntimeError, e:
rospy.logerr("Exception while getting life state:\n%s", e)
return None
def look_at_default_cb(self, req):
resp = TriggerResponse()
pt = PointStamped()
pt.header.frame_id = "base_link"
pt.point.x = 0.4
pt.point.y = -0.15
pt.point.z = 0.8
self.look_at_cb(pt, send_and_wait=False)
resp.success = True
return resp
def look_at_right_feeder_cb(self, req):
resp = TriggerResponse()
point = PointStamped()
point.header.frame_id = "marker"
point.point.x = 1.825
point.point.y = 0.205
point.point.z = 0.0875
self.look_at_cb(point, send_and_wait=False)
resp.success = True
return resp
def left_interaction_off_cb(self, req):
resp = TriggerResponse()
if not self.left_arm_mann:
rospy.logerr('Left arm already in normal mode')
resp.success = True
else:
self.switch_req.stop_controllers = [self.mannequin_controllers[1]]
self.switch_req.start_controllers = [self.standard_controllers[1]]
res = self.switch_control(self.switch_req)
if res.ok:
self.left_arm_mann = False
self.left_int_pub.publish(False)
resp.success = True
else:
resp.success = False
resp.message = "Left arm: failed to switch interaction to OFF"
return resp
def right_interaction_on_cb(self, req):
resp = TriggerResponse()
if self.right_arm_mann:
rospy.logerr('Right arm already in interactive mode')
resp.success = True
else:
self.switch_req.stop_controllers = [self.standard_controllers[0]]
self.switch_req.start_controllers = [self.mannequin_controllers[0]]
res = self.switch_control(self.switch_req)
if res.ok:
self.right_arm_mann = True
self.right_int_pub.publish(True)
resp.success = True
else:
resp.success = False
resp.message = "Left arm: failed to switch interaction to ON"
return resp
def server_callback(req):
rospy.loginfo('Server callback!')
resp = TriggerResponse()
resp.success = True
resp.message = 'OK'
return resp
def __is_status_paused(request):
response = TriggerResponse()
response.success = (CodeStatus.PAUSED == CodeStatus.get_current_status())
return response
