def flight_init(self):
#https://github.com/ethz-asl/rotors_simulator/blob/master/rqt_rotors/src/rqt_rotors/hil_plugin.py
#https://github.com/PX4/Firmware/blob/master/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py
#https://github.com/PX4/Firmware/blob/master/integrationtests/python_src/px4_it/mavros/mavros_test_common.py
rospy.loginfo("============flight_init===============")
for i in range(5):
try:
for i in range(10):
self.moveto(0,0,self.H)
self.ros_delay(0.1)
res = self.set_arm(True)
if not res.success:
raise rospy.ServiceException("failed to send ARM command")
continue
res = self.set_mode(0,"OFFBOARD")
if not res.mode_sent:
raise rospy.ServiceException("failed to send OFFBOARD command")
continue
except rospy.ServiceException as e:
rospy.logerr(e)
continue
return True
return False
def assign_descriptor_to_lama_object(self, msg):
"""Add a descriptor to a vertex
Callback for ASSIGN_DESCRIPTOR_VERTEX and ASSIGN_DESCRIPTOR_EDGE.
Return an instance of ActOnMap response.
Parameters
----------
- msg: an instance of ActOnMap request.
"""
# Ensure that the lama object exists in the core table.
object_id = msg.object.id
core_table = self.core_iface.core_table
query = core_table.select(whereclause=(core_table.c.id == object_id))
connection = self.core_iface.engine.connect()
with connection.begin():
result = connection.execute(query).fetchone()
connection.close()
if not result:
err = 'No lama object with id {} in database table {}'.format(
object_id, core_table.name)
raise rospy.ServiceException(err)
# Ensure that the descriptor exists in the database.
if not msg.interface_name:
raise rospy.ServiceException('Missing interface name')
table_name = msg.interface_name
if not self.core_iface.has_table(table_name):
err = 'No interface "{}" in the database'.format(
msg.interface_name)
raise rospy.ServiceException(err)
table = self.core_iface.metadata.tables[table_name]
desc_id = msg.descriptor_id
query = table.select(whereclause=(table.c.id == desc_id))
connection = self.core_iface.engine.connect()
with connection.begin():
result = connection.execute(query).fetchone()
connection.close()
if not result:
err = 'No descriptor with id {} in database table {}'.format(
desc_id, table.name)
raise rospy.ServiceException(err)
# Add the descriptor to the descriptor table.
time = rospy.Time.now()
insert_args = {
'object_id': object_id,
'descriptor_id': desc_id,
'interface_name': table_name,
'timestamp_secs': time.secs,
'timestamp_nsecs': time.nsecs,
}
connection = self.core_iface.engine.connect()
with connection.begin():
connection.execute(self.core_iface.descriptor_table.insert(),
insert_args)
connection.close()
response = ActOnMapResponse()
return response
def set_workspace_shape(self, req):
ray_tf = self.pointing_model.pointing_ray()
req_ws_shape = self.ws_shape
if req.robot_frame:
self.robot_frame = req.robot_frame
if ray_tf:
ray_kdl_frame = transform_to_kdl(ray_tf)
robot_kdl_frame = self.frame_from_tf(self.human_frame, self.robot_frame) #self.ws_shape.frame_id, self.robot_frame)
ray_origin_kdl_frame = self.frame_from_tf(self.human_frame, self.ray_origin_frame)
if robot_kdl_frame and ray_origin_kdl_frame:
if req.workspace_shape == SetWorkspaceShapeRequest.WORKSPACE_XY_PLANE:
req_ws_shape = self.xy_plane
elif req.workspace_shape == SetWorkspaceShapeRequest.WORKSPACE_VISUAL_PLANE:
req_ws_shape = self.vis_plane
elif req.workspace_shape == SetWorkspaceShapeRequest.WORKSPACE_CYLINDER:
req_ws_shape = self.cylinder
elif req.workspace_shape == SetWorkspaceShapeRequest.WORKSPACE_SPHERE:
req_ws_shape = self.sphere
else:
raise rospy.ServiceException('Unsupported shape type')
# return None
cur_pointer_pose = self.get_pointer(self.ws_shape, ray_kdl_frame, self.ws_shape.point)
if self.switch_at_pointer:
switch_point = copy.deepcopy(tfc.fromMsg(cur_pointer_pose).p)
else:
switch_point = copy.deepcopy(robot_kdl_frame.p)
new_pointer_pose = self.get_pointer(req_ws_shape, ray_kdl_frame, switch_point)
eyes_robot_vector = kdl.Vector(robot_kdl_frame.p - ray_origin_kdl_frame.p)
robot_elev_frame = self.pointing_model._frame_from_direction(ray_origin_kdl_frame.p, eyes_robot_vector)
_, pitch, _ = robot_elev_frame.M.GetRPY()
if math.fabs(pitch) < self.min_elevation_angle:
raise rospy.ServiceException('Drone elevation angle is less than {} deg: {}. Ignoring'
.format(math.degrees(self.min_elevation_angle), math.degrees(pitch)))
else:
# Since the safety check succeeded we can update the pass_point of the shape
new_pointer_pose = self.get_pointer(req_ws_shape, ray_kdl_frame, switch_point, cache = True)
else:
raise rospy.ServiceException('Unable to obtain robot\'s frame. Is robot localized?')
# return None
else:
raise rospy.ServiceException('Internal error: failed to get ray frame')
# return None
self.ws_shape = req_ws_shape
return SetWorkspaceShapeResponse()
def read_images(self, req):
"""Reads images from a ROS service request.
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS ServiceRequest for grasp planner service.
"""
# Get the raw depth and color images as ROS `Image` objects.
raw_color = req.color_image
raw_depth = req.depth_image
# Get the raw camera info as ROS `CameraInfo`.
raw_camera_info = req.camera_info
# Wrap the camera info in a BerkeleyAutomation/perception
# `CameraIntrinsics` object.
camera_intr = CameraIntrinsics(
raw_camera_info.header.frame_id, raw_camera_info.K[0],
raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5],
raw_camera_info.K[1], raw_camera_info.height,
raw_camera_info.width)
# Create wrapped BerkeleyAutomation/perception RGB and depth images by
# unpacking the ROS images using ROS `CvBridge`
try:
color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2(
raw_color, "rgb8"),
frame=camera_intr.frame)
depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2(
raw_depth, desired_encoding="passthrough"),
frame=camera_intr.frame)
except CvBridgeError as cv_bridge_exception:
rospy.logerr(cv_bridge_exception)
# Check image sizes.
if color_im.height != depth_im.height or \
color_im.width != depth_im.width:
msg = ("Color image and depth image must be the same shape! Color"
" is %d x %d but depth is %d x %d") % (
color_im.height, color_im.width, depth_im.height,
depth_im.width)
rospy.logerr(msg)
raise rospy.ServiceException(msg)
if (color_im.height < self.min_height
or color_im.width < self.min_width):
msg = ("Color image is too small! Must be at least %d x %d"
" resolution but the requested image is only %d x %d") % (
self.min_height, self.min_width, color_im.height,
color_im.width)
rospy.logerr(msg)
raise rospy.ServiceException(msg)
return color_im, depth_im, camera_intr
def __set_pose(self, request):
"""Set the passed in pose in the database. If the default value for the
id of 0 is used, a new entry will be created. If the value of an
existing row's id is passed, that row will be replaced with the new
data. If a non-zero id is passed which does not exist in the database,
an error will be raised.
:param request: The SetPose service message request
"""
db = DB(self.db_path) # pylint: disable=invalid-name
update = DBUpdate()
if not len(request.pose.joint_positions) == len(request.pose.joint_names):
raise rospy.ServiceException(
'the length of the pose values and names are not consistent')
if request.id:
curs = db.ex('select id from poses where id = ?', request.id)
data = curs.fetchone()
if data:
db_return = db.ex(
'replace into poses('
+ 'id, description, joint_positions, joint_names)'
+ 'values (?,?,?,?)',
request.id,
request.pose.description,
json.dumps(request.pose.joint_positions),
json.dumps(request.pose.joint_names))
updated_row = request.id
rospy.loginfo('updated pose at id: %i', updated_row)
update.command = DBUpdate.UPDATE
else:
rospy.logerr('Attempt to change a non-existant row')
raise rospy.ServiceException(
'The selected row does not exist, you cannot update it')
else:
db_return = db.ex(
'insert into poses(description, joint_positions, joint_names)'
+ 'values (?,?,?)',
request.pose.description,
json.dumps(request.pose.joint_positions),
json.dumps(request.pose.joint_names))
updated_row = db_return.lastrowid
rospy.loginfo('stored new pose at id: %i', updated_row)
update.command = DBUpdate.CREATE
update.table = DBUpdate.POSES
update.value = json.dumps(
{'description': request.pose.description,
'joint_positions': request.pose.joint_positions,
'joint_names': request.pose.joint_names})
self.db_updates.publish(update)
update.id = updated_row
return updated_row
def read_images(self, req):
""" Reads images from a ROS service request
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS ServiceRequest for grasp planner service
"""
# get the raw depth and color images as ROS Image objects
raw_color = req.color_image
raw_depth = req.depth_image
# get the raw camera info as ROS CameraInfo object
raw_camera_info = req.camera_info
# wrap the camera info in a perception CameraIntrinsics object
camera_intr = CameraIntrinsics(
raw_camera_info.header.frame_id, raw_camera_info.K[0],
raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5],
raw_camera_info.K[1], raw_camera_info.height,
raw_camera_info.width)
# create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ###
try:
color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2(
raw_color, "rgb8"),
frame=camera_intr.frame)
depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2(
raw_depth, desired_encoding="passthrough"),
frame=camera_intr.frame)
except CvBridgeError as cv_bridge_exception:
rospy.logerr(cv_bridge_exception)
# check image sizes
if color_im.height != depth_im.height or \
color_im.width != depth_im.width:
msg = 'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d' % (
color_im.height, color_im.width, depth_im.height,
depth_im.width)
rospy.logerr(msg)
raise rospy.ServiceException(msg)
if color_im.height < self.min_height or color_im.width < self.min_width:
msg = 'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d' % (
self.min_height, self.min_width, color_im.height,
color_im.width)
rospy.logerr(msg)
raise rospy.ServiceException(msg)
return color_im, depth_im, camera_intr
def recognize_srv_callback(self, req):
"""
Method callback for the Recognize.srv
:param req: The service request
"""
self._response.recognitions = []
self._recognizing = True
try:
cv_image = self.bridge.imgmsg_to_cv2(req.image, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
self._image_widget.set_image(cv_image)
self._done_recognizing_button.setDisabled(False)
timeout = 60.0 # Maximum of 60 seconds
future = rospy.Time.now() + rospy.Duration(timeout)
rospy.loginfo("Waiting for manual recognition, maximum of %d seconds", timeout)
while not rospy.is_shutdown() and self._recognizing:
if rospy.Time.now() > future:
raise rospy.ServiceException("Timeout of %d seconds exceeded .." % timeout)
rospy.sleep(rospy.Duration(0.1))
self._done_recognizing_button.setDisabled(True)
return self._response
def __get_game_bucket_id(self, request):
"""Get a game bucket given its id
Args:
request: the service request
Returns:
The service response
"""
db = DB(self.db_path) # pylint: disable=invalid-name
curs = db.ex('select * from game_buckets where id = ?', request.id)
data = curs.fetchone()
if data:
resp = GetGameBucketIDResponse()
game_bucket = GameBucket()
resp.id = data['id']
game_bucket.name = data['name']
game_bucket.subject = data['subject']
game_bucket.targeted_game = data['targeted_game']
game_bucket.description = data['description']
dirty_steps = json.loads(data['steps'])
clean_steps = []
for step in dirty_steps:
clean_steps.append(
StepDef(type=step['type'], text=step['text'],
id=step['id'], time=step['time']))
game_bucket.steps = clean_steps
resp.game_bucket = game_bucket
return resp
raise rospy.ServiceException('That ID does not exist')
def begin_trial ( self, request ) :
if self.output_directory is not None :
return "trial is already in progress."
else :
self.output_directory = request.directory
now = datetime.date.fromtimestamp( time.time() )
self.output_directory += '_' + str(now)
trial_count = 1
if os.path.exists(self.output_directory) :
for name in os.listdir(self.output_directory) :
if name.rfind('trial') >= 0 :
trial_count += 1
self.output_directory += '/trial_{}'.format(trial_count)
try :
os.makedirs ( self.output_directory )
os.chdir ( self.output_directory )
return "ok, trial started at {}".format( self.output_directory )
except OSError as e:
error_string = str(e)
raise ( rospy.ServiceException(error_string) )
def _cb_set_stop_force(self, req):
if req.axis in (req.AXIS_X, req.AXIS_Y, req.AXIS_Z):
threshold = getattr(self._current_wrench.force, req.axis) + req.val
info = 'axis: {}, val: {}, threshold: {}'.format(
req.axis, req.val, threshold)
rospy.loginfo('set_stop_force: {}'.format(info))
if req.val >= 0:
self.add_handler(
lambda msg: getattr(msg.wrench.force, req.axis) >
threshold, lambda msg: self.stop(), info)
else:
self.add_handler(
lambda msg: getattr(msg.wrench.force, req.axis) <
threshold, lambda msg: self.stop(), info)
elif req.axis in (req.AXIS_MX, req.AXIS_MY, req.AXIS_MZ):
threshold = getattr(self._current_wrench.torque,
req.axis) + req.val
info = 'axis: {}, val: {}, threshold: {}'.format(
req.axis, req.val, threshold)
rospy.loginfo('set_stop_force: {}'.format(info))
if req.val >= 0:
self.add_handler(
lambda msg: getattr(msg.wrench.torque, req.axis) >
threshold, lambda msg: self.stop(), info)
else:
self.add_handler(
lambda msg: getattr(msg.wrench.torque, req.axis) <
threshold, lambda msg: self.stop(), info)
else:
raise rospy.ServiceException('invalid axis: {}'.format(req.axis))
res = SetStopForceResponse()
return res
def handle_request(req):
try:
gripper = get_current_gripper_interface()
is_clicksmart = isinstance(gripper, SimpleClickSmartGripper)
if is_clicksmart:
if gripper.needs_init():
gripper.initialize()
else:
if not (gripper.is_calibrated() or
gripper.calibrate() == True):
raise
return
if(req.command=="open"):
try:
gripper.set_ee_signal_value('grip', True)
print "Gripper is open"
except Exception as e:
print e
return OpenGripperResponse(True)
elif(req.command=="close"):
try:
gripper.set_ee_signal_value('grip', False)
print "Gripper is closed"
except Exception as e:
print e
return OpenGripperResponse(False)
except:
gripper = None
is_clicksmart = False
else:
raise rospy.ServiceException("invalid command")
def test_rosservicewrapper_call(self, service_proxy_mock):
client = ROSCLEClient.ROSCLEClient(0)
# When everything is fine, the service should just be called
service_wrapper = ROSCLEClient.ROSCLEServiceWrapper(
'service_name', Empty, client)
self.assertEqual(client, service_wrapper.ros_cle_client)
service_wrapper()
self.assertEquals(service_wrapper.handler.call_count, 1)
# When something's wrong during the service call it should throw ROSCLEClientException
service_wrapper = ROSCLEClient.ROSCLEServiceWrapper(
'service_name', Empty, client, invalidate_on_failure=False)
service_wrapper.handler.side_effect = rospy.ServiceException()
self.assertRaises(ROSCLEClient.ROSCLEClientException, service_wrapper)
service_wrapper.handler.side_effect = rospy.exceptions.ROSInterruptException(
)
self.assertRaises(ROSCLEClient.ROSCLEClientException, service_wrapper)
# and invalidate the ROSCLEClient if asked to
service_wrapper = ROSCLEClient.ROSCLEServiceWrapper(
'service_name', Empty, client, invalidate_on_failure=True)
self.assertRaises(ROSCLEClient.ROSCLEClientException, service_wrapper)
def detect_objects_cb(request):
rospy.loginfo('Received [detect_objects] request.')
outcome = sm.execute()
if outcome == 'succeeded':
return sm.userdata.response
else:
raise rospy.ServiceException('Object detection failed')
def handle_get_wind(self, req):
"""Handle get wind request for position.
Arguments:
req: GetWindRequest.
Returns:
GetWindResponse containing real wind in position.
Raises:
rospy.ServiceException with the error
"""
rospy.logdebug("[atmosphere] [ac_id={}] get_wind request: "
"time={} position={}".format(
req.ac_id, req.time.to_time(), req.position))
try:
w = self.get_wind(req.position.x, req.position.y, req.position.z,
req.time.to_time())
wind = Wind(w[0], w[1], w[2])
rospy.logdebug(
"[atmosphere] [ac_id={}] get_wind response:\n{}".format(
req.ac_id, wind))
return GetWindResponse(wind)
except Exception as e:
rospy.logerr(e)
raise rospy.ServiceException(e)
def plan_grasp_segmask(self, req):
"""Grasp planner request handler.
Parameters
---------
req: :obj:`ROS ServiceRequest`
ROS `ServiceRequest` for grasp planner service.
"""
color_im, depth_im, camera_intr = self.read_images(req)
raw_segmask = req.segmask
try:
segmask = BinaryImage(self.cv_bridge.imgmsg_to_cv2(
raw_segmask, desired_encoding="passthrough"),
frame=camera_intr.frame)
except CvBridgeError as cv_bridge_exception:
rospy.logerr(cv_bridge_exception)
if color_im.height != segmask.height or \
color_im.width != segmask.width:
msg = ("Images and segmask must be the same shape! Color image is"
" %d x %d but segmask is %d x %d") % (
color_im.height, color_im.width, segmask.height,
segmask.width)
rospy.logerr(msg)
raise rospy.ServiceException(msg)
return self._plan_grasp(color_im,
depth_im,
camera_intr,
segmask=segmask)
def _get_score(self, request):
# TODO: (Optional) Check that the location is in bounds, and raise a
# rospy.ServiceException if not.
if (request.row >= self._world.num_rows()
or request.col >= self._world.num_cols()):
raise rospy.ServiceException('Row or column is out of bounds.')
# TODO: Return the score for the correct row and column.
return self._world.get_score(request.row, request.col)
def action_callback(self, msg):
"""Callback of ActOnMap service
Return an instance of ActOnMap response.
Parameters
----------
- msg: an instance of ActOnMap request.
"""
# Raise an error if pushing the wrong type.
if ((msg.action == msg.PUSH_VERTEX)
and (msg.object.type not in [0, msg.object.VERTEX])):
raise rospy.ServiceException(
'Action PUSH_VERTEX, LamaObject is not a vertex')
if ((msg.action == msg.PUSH_EDGE)
and (msg.object.type not in [0, msg.object.EDGE])):
raise rospy.ServiceException(
'Action PUSH_EDGE, LamaObject is not an edge')
# Force object type for PUSH_VERTEX and PUSH_EDGE.
if msg.action == msg.PUSH_EDGE:
msg.object.type = msg.object.EDGE
if msg.action == msg.PUSH_VERTEX:
msg.object.type = msg.object.VERTEX
callbacks = {
msg.PUSH_VERTEX: self.push_lama_object,
msg.PULL_VERTEX: self.pull_lama_object,
msg.ASSIGN_DESCRIPTOR_VERTEX:
(self.assign_descriptor_to_lama_object),
msg.PUSH_EDGE: self.push_lama_object,
msg.PULL_EDGE: self.pull_lama_object,
msg.ASSIGN_DESCRIPTOR_EDGE:
(self.assign_descriptor_to_lama_object),
msg.GET_VERTEX_LIST: self.get_vertex_list,
msg.GET_EDGE_LIST: self.get_edge_list,
msg.GET_DESCRIPTOR_LINKS: self.get_descriptor_links,
msg.GET_NEIGHBOR_VERTICES: self.get_neighbor_vertices,
msg.GET_OUTGOING_EDGES: self.get_outgoing_edges,
}
if msg.action not in callbacks:
raise rospy.ServiceException('Action {} not implemented'.format(
msg.action))
callback = callbacks[msg.action]
response = callback(msg)
return response
def request_area(w,h):
rospy.wait_for_service('Rec_area')
try:
area = rospy.ServiceProxy('Rec_area',RectangleAreaService)
resp= area(w,h)
return resp.area
except rospy.ServiceException(e):
print("Service call failled : %s"%e)
def handle_get_landmark(req):
if not req.name in wps:
raise rospy.ServiceException("Node " + req.name + " not found.")
return GetLandmarkResponse(
Landmark(x=wps[req.name][0],
y=wps[req.name][1],
ltype=wps[req.name][2],
name=req.name))
def get_rectangle_area(x, y):
rospy.wait_for_service('rectangle_area_service')
try:
multiply_two_ints = rospy.ServiceProxy('rectangle_area_service', RectangleAreaService)
response = multiply_two_ints(x, y)
return response.area
except rospy.ServiceException(e):
print ("Service call failed: %s"%e)
def register_obstacles(self, num_obstacles: int, model_yaml_file_path: str, type_obstacle: str):
"""register the static obstacles and request flatland to respawn the them.
Args:
num_obstacles (string): the number of the obstacle instance to be created.
model_yaml_file_path (string or None): model file path. it must be absolute path!
type_obstacle (string or None): type of the obstacle. it must be 'dynamic' or 'static'.
Raises:
Exception: Rospy.ServiceException(
f" failed to register obstacles")
Returns:
self.
"""
assert os.path.isabs(
model_yaml_file_path), "The yaml file path must be absolute path, otherwise flatland can't find it"
assert type_obstacle == 'dynamic' or type_obstacle == 'static', 'The type of the obstacle must be dynamic or static'
# the name of the model yaml file have the format {model_name}.model.yaml
type_obstacle = self._obstacle_name_prefix+'_'+type_obstacle
model_name = os.path.basename(model_yaml_file_path).split('.')[0]
name_prefix = type_obstacle + '_' + model_name
count_same_type = sum(
1 if obstacle_name.startswith(name_prefix) else 0
for obstacle_name in self.obstacle_name_list)
for instance_idx in range(count_same_type, count_same_type + num_obstacles):
max_num_try = 2
i_curr_try = 0
while i_curr_try < max_num_try:
spawn_request = SpawnModelRequest()
spawn_request.yaml_path = model_yaml_file_path
spawn_request.name = f'{name_prefix}_{instance_idx:02d}'
spawn_request.ns = rospy.get_namespace()
# x, y, theta = get_random_pos_on_map(self._free_space_indices, self.map,)
# set the postion of the obstacle out of the map to hidden them
x = self.map.info.origin.position.x - 3 * \
self.map.info.resolution * self.map.info.height
y = self.map.info.origin.position.y - 3 * \
self.map.info.resolution * self.map.info.width
theta = theta = random.uniform(-math.pi, math.pi)
spawn_request.pose.x = x
spawn_request.pose.y = y
spawn_request.pose.theta = theta
# try to call service
response = self._srv_spawn_model.call(spawn_request)
if not response.success: # if service not succeeds, do something and redo service
rospy.logwarn(
f"spawn object {spawn_request.name} failed! trying again... [{i_curr_try+1}/{max_num_try} tried]")
rospy.logwarn(response.message)
i_curr_try += 1
else:
self.obstacle_name_list.append(spawn_request.name)
break
if i_curr_try == max_num_try:
raise rospy.ServiceException(f" failed to register obstacles")
return self
def set_start_pos_goal_pos(self, start_pos: Union[Pose2D, None]
= None, goal_pos: Union[Pose2D, None] = None, min_dist=1):
"""set up start position and the goal postion. Path validation checking will be conducted. If it failed, an
exception will be raised.
Args:
start_pos (Union[Pose2D,None], optional): start position. if None, it will be set randomly. Defaults to None.
goal_pos (Union[Pose2D,None], optional): [description]. if None, it will be set randomly .Defaults to None.
min_dist (float): minimum distance between start_pos and goal_pos
Exception:
Exception("can not generate a path with the given start position and the goal position of the robot")
"""
def dist(x1, y1, x2, y2):
return math.sqrt((x1 - x2)**2 + (y1 - y2)**2)
if start_pos is None or goal_pos is None:
# if any of them need to be random generated, we set a higher threshold,otherwise only try once
max_try_times = 20
else:
max_try_times = 1
i_try = 0
start_pos_ = None
goal_pos_ = None
while i_try < max_try_times:
if start_pos is None:
start_pos_ = Pose2D()
start_pos_.x, start_pos_.y, start_pos_.theta = get_random_pos_on_map(
self._free_space_indices, self.map, self.ROBOT_RADIUS * 2)
else:
start_pos_ = start_pos
if goal_pos is None:
goal_pos_ = Pose2D()
goal_pos_.x, goal_pos_.y, goal_pos_.theta = get_random_pos_on_map(
self._free_space_indices, self.map, self.ROBOT_RADIUS * 4)
else:
goal_pos_ = goal_pos
if dist(start_pos_.x, start_pos_.y, goal_pos_.x, goal_pos_.y) < min_dist:
i_try += 1
continue
# move the robot to the start pos
self.move_robot(start_pos_)
try:
# publish the goal, if the gobal plath planner can't generate a path, a, exception will be raised.
self.publish_goal(goal_pos_.x, goal_pos_.y, goal_pos_.theta)
break
except rospy.ServiceException:
i_try += 1
if i_try == max_try_times:
# TODO Define specific type of Exception
raise rospy.ServiceException(
"can not generate a path with the given start position and the goal position of the robot")
else:
return start_pos_, goal_pos_
def test_worldSDF_service_serviceProxy_fail(self, mckd_ServiceProxy, _):
mckd_ServiceProxy.return_value = mock.MagicMock(
side_effect=rospy.ServiceException('Mocked ServiceException'))
# call the service
response = self.client.get('/simulation/0/sdf_world')
# check the status code
self.assertEqual(response.status_code, 400)
def __set_utterance(self, request):
"""Set an utterance in the Database
Updates (if given an ID) or creates a new utterance
(if not given and ID)
based on the request passed from the action server
Args:
request: The action server request
Returns: The action server response
"""
db = DB(self.db_path) # pylint: disable=invalid-name
update = DBUpdate()
# Find the length of the utterance:
mut_d = mutagen.File(request.filename)
time_length = mut_d.info.length
if request.id:
curs = db.ex('select id from utterances where id = ?', request.id)
data = curs.fetchone()
if data:
db_return = db.ex(
'replace into utterances(id, text, length, metadata) '
+ 'values (?,?,?,?)',
request.id,
request.text,
time_length,
request.metadata
)
updated_row = request.id
rospy.loginfo('updated utterance at id: %i', updated_row)
update.command = DBUpdate.UPDATE
else:
rospy.logerr('Attempt to change a non-existant row')
raise rospy.ServiceException(
'The selected row does not exist, you cannot update it')
else:
db_return = db.ex(
'insert into utterances(text, length, metadata) '
+ 'values (?,?,?)',
request.text,
time_length,
request.metadata
)
updated_row = db_return.lastrowid
rospy.loginfo('stored new utterance at id: %i', updated_row)
update.command = DBUpdate.CREATE
update.id = updated_row
update.table = DBUpdate.UTTERANCES
update.command = json.dumps({
'text': request.text,
'length': time_length,
'metadata': request.metadata
})
resp = SetUtteranceResponse(id=updated_row, time=time_length)
return resp
def init_reg_cb(self, req):
# TODO REMOVE THIS FACE SIDE MESS
self.publish_feedback("Performing Head Registration. Please Wait.")
self.face_side = rospy.get_param("/face_side", 'r')
bag_str = self.reg_dir + '/' + '_'.join(
[self.subject, self.face_side, "head_transform"]) + ".bag"
rospy.loginfo("[%s] Loading %s" % (rospy.get_name(), bag_str))
try:
bag = rosbag.Bag(bag_str, 'r')
for topic, msg, ts in bag.read_messages():
head_tf = msg
assert (head_tf
is not None), "Error reading head transform bagfile"
bag.close()
except Exception as e:
self.publish_feedback(
"Registration failed: Error loading saved registration.")
rospy.logerr(
"[%s] Cannot load registration parameters from %s:\r\n%s" %
(rospy.get_name(), bag_str, e))
return (False, PoseStamped())
if self.face_side == 'r':
head_registration = self.head_registration_r
else:
head_registration = self.head_registration_l
try:
rospy.loginfo(
"[%s] Requesting head registration for %s at pixel (%d, %d)." %
(rospy.get_name(), self.subject, req.u, req.v))
self.head_pc_reg = head_registration(req.u, req.v).reg_pose
if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0))
and (self.head_pc_reg.pose.orientation == Quaternion(
0.0, 0.0, 0.0, 1.0))):
raise rospy.ServiceException("Unable to find a good match.")
self.head_pc_reg = None
except rospy.ServiceException as se:
self.publish_feedback("Registration failed: %s" % se)
return (False, PoseStamped())
pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
head_tf_mat = PoseConv.to_homo_mat(
Transform(head_tf.transform.translation,
head_tf.transform.rotation))
self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 *
head_tf_mat)
self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
self.head_pose.header.stamp = self.head_pc_reg.header.stamp
side = "right" if (self.face_side == 'r') else "left"
self.publish_feedback(
"Registered head using %s cheek model, please check and confirm." %
side)
# rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
self.test_pose.publish(self.head_pose)
return (True, self.head_pose)
def set_lama_object(self, lama_object):
"""Add/modify a lama object to the database
Return the lama object's id.
Parameter
---------
- lama_object: an instance of LamaObject.
"""
if len(lama_object.references) != 2:
raise rospy.ServiceException(
'malformed references, length = {}'.format(
len(lama_object.references)))
if ((lama_object.id == 0) and
(lama_object.type == LamaObject.EDGE) and
(0 in lama_object.references)):
# 0 is undefined and not allowed.
raise rospy.ServiceException('edge references cannot be 0')
is_undefined = (lama_object.name == 'undefined')
is_special_vertex = lama_object.id < 0 or is_undefined
is_new_vertex = lama_object.id == 0 and not is_undefined
# Check for id existence.
query = self.core_table.select(
whereclause=(self.core_table.c.id == lama_object.id))
connection = self.engine.connect()
with connection.begin():
result = connection.execute(query).fetchone()
connection.close()
if result is not None and is_special_vertex:
# Exit if lama_object is an already-existing special object.
return lama_object.id
if result is not None and not is_new_vertex:
# Update existing Lama Object.
object_id = self._update_lama_object(lama_object)
else:
# Insert new Lama Object.
object_id = self._insert_lama_object(lama_object)
return object_id
def set_offboard_mode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode',
mavros_msgs.srv.SetMode)
flightModeService(custom_mode='OFFBOARD')
except rospy.ServiceException(e):
print(
"service set_mode call failed: %s. Offboard Mode could not be set."
% e)
def set_arming(self):
rospy.wait_for_service('/mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('/mavros/cmd/arming',
mavros_msgs.srv.CommandBool)
armService(True)
except rospy.ServiceException(e):
print(
"service set_mode call failed: %s. Offboard Mode could not be set."
% e)
def execute_policy(self, rgbd_image_state, grasping_policy,
grasp_pose_publisher, pose_frame):
"""Executes a grasping policy on an `RgbdImageState`.
Parameters
----------
rgbd_image_state: :obj:`RgbdImageState`
`RgbdImageState` from BerkeleyAutomation/perception to encapsulate
depth and color image along with camera intrinsics.
grasping_policy: :obj:`GraspingPolicy`
Grasping policy to use.
grasp_pose_publisher: :obj:`Publisher`
ROS publisher to publish pose of planned grasp for visualization.
pose_frame: :obj:`str`
Frame of reference to publish pose in.
"""
# Execute the policy"s action.
grasp_planning_start_time = time.time()
grasp = grasping_policy(rgbd_image_state)
# Create `GQCNNGrasp` return msg and populate it.
gqcnn_grasp = GQCNNGrasp()
gqcnn_grasp.q_value = grasp.q_value
gqcnn_grasp.pose = grasp.grasp.pose().pose_msg
if isinstance(grasp.grasp, Grasp2D):
gqcnn_grasp.grasp_type = GQCNNGrasp.PARALLEL_JAW
elif isinstance(grasp.grasp, SuctionPoint2D):
gqcnn_grasp.grasp_type = GQCNNGrasp.SUCTION
else:
rospy.logerr("Grasp type not supported!")
raise rospy.ServiceException("Grasp type not supported!")
# Store grasp representation in image space.
gqcnn_grasp.center_px[0] = grasp.grasp.center[0]
gqcnn_grasp.center_px[1] = grasp.grasp.center[1]
gqcnn_grasp.angle = grasp.grasp.angle
gqcnn_grasp.depth = grasp.grasp.depth
gqcnn_grasp.thumbnail = grasp.image.rosmsg
# Create and publish the pose alone for easy visualization of grasp
# pose in Rviz.
pose_stamped = PoseStamped()
pose_stamped.pose = grasp.grasp.pose().pose_msg
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = pose_frame
pose_stamped.header = header
grasp_pose_publisher.publish(pose_stamped)
# Return `GQCNNGrasp` msg.
rospy.loginfo("Total grasp planning time: " +
str(time.time() - grasp_planning_start_time) + " secs.")
return gqcnn_grasp
def confirm_reg_cb(self, req):
if self.head_pose is None:
raise rospy.ServiceException("Head has not been registered.")
return False
try:
hp = copy.copy(self.head_pose)
now = rospy.Time.now() + rospy.Duration(0.5)
self.tfl.waitForTransform(self.WORLD_FRAME, hp.header.frame_id,
now, rospy.Duration(10))
hp.header.stamp = now
hp_world = self.tfl.transformPose(self.WORLD_FRAME, hp)
pos = (hp_world.pose.position.x, hp_world.pose.position.y,
hp_world.pose.position.z)
quat = (hp_world.pose.orientation.x, hp_world.pose.orientation.y,
hp_world.pose.orientation.z, hp_world.pose.orientation.w)
self.head_frame_tf = (pos, quat)
self.publish_feedback("Head registration confirmed.")
return True
except Exception as e:
rospy.logerr("[%s] Error: %s" % (rospy.get_name(), e))
raise rospy.ServiceException("Error confirming head registration.")
