def wait_for_msg(self, timeout=2.0):
end = rospy.Time.now() + rospy.Duration(timeout)
while rospy.Time.now() < end:
if self.last_msg is not None:
return
rospy.sleep(0.1)
raise rospy.ROSException("Timeout waiting for msg")
def compute_force(self,
acc=None,
vel=None,
with_restoring=True,
use_sname=True):
"""Return the sum of forces acting on the vehicle.
Given acceleration and velocity vectors, this function returns the
sum of forces given the rigid-body and hydrodynamic models for the
marine vessel.
"""
if acc is not None:
if acc.shape != (6, ):
raise rospy.ROSException('Acceleration vector must have 6 '
'elements')
# It is assumed the input acceleration is given in the SNAME convention
nu_dot = acc
else:
# Convert the acceleration vector to the SNAME convention since the odometry is usually given using the
# ENU convention
nu_dot = self.to_SNAME(self._acc)
if vel is not None:
if vel.shape != (6, ):
raise rospy.ROSException('Velocity vector must have 6 '
'elements')
# It is assumed the input velocity is given in the SNAME convention
nu = vel
else:
nu = self.to_SNAME(self._vel)
self._update_damping(nu)
self._update_coriolis(nu)
self._update_restoring(use_sname=True)
if with_restoring:
g = deepcopy(self._g)
else:
g = np.zeros(6)
f = np.dot(self._Mtotal, nu_dot) + np.dot(self._C, nu) + \
np.dot(self._D, nu) + g
if not use_sname:
f = self.from_SNAME(f)
return f
def __init__(self):
DPControllerBase.__init__(self, True)
self._tau = np.zeros(6)
self._logger.info('Initializing: ' + self._LABEL)
self._Kd = np.zeros(shape=(6, 6))
if rospy.has_param('~Kd'):
coefs = rospy.get_param('~Kd')
if len(coefs) == 6:
self._Kd = np.diag(coefs)
else:
raise rospy.ROSException('Kd coefficients: 6 coefficients '
'needed')
self._logger.info('Kd=\n' + str(self._Kd))
# Build delta matrix
self._delta = np.zeros(shape=(6, 6))
l = rospy.get_param('~lambda', [0.0])
if len(l) == 1:
self._delta[0:3, 0:3] = l[0] * np.eye(3)
elif len(l) == 3:
self._delta[0:3, 0:3] = np.diag(l)
else:
raise rospy.ROSException(
'lambda: either a scalar or a 3 element vector must be provided')
c = rospy.get_param('~c', [0.0])
if len(c) == 1:
self._delta[3:6, 3:6] = c[0] * np.eye(3)
elif len(c) == 3:
self._delta[3:6, 3:6] = np.diag(c)
else:
raise rospy.ROSException(
'c: either a scalar or a 3 element vector must be provided')
self._logger.info('delta=\n' + str(self._delta))
self._prev_t = None
self._prev_vel_r = np.zeros(6)
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__(self):
print('AccelerationControllerNode: initializing node')
self.vehicle_model = Vehicle()
self.ready = False
self.mass = 1.
self.inertial_tensor = numpy.identity(3)
self.mass_inertial_matrix = numpy.zeros((6, 6))
self.vel_ = numpy.zeros(6)
self.acc_ = numpy.zeros(6)
# ROS infrastructure
self.pub_gen_force = rospy.Publisher('thruster_manager/input',
Wrench,
queue_size=1)
self.sub_accel = rospy.Subscriber('cmd_accel', numpy_msg(Accel),
self.accel_callback)
self.sub_vel = rospy.Subscriber('/anahita/pose_gt',
numpy_msg(Odometry), self.vel_callback)
self.sub_force = rospy.Subscriber('cmd_force', numpy_msg(Accel),
self.force_callback)
"""self.pub_gen_force = rospy.Publisher(
'thruster_manager/input', Wrench, queue_size=1)"""
if not rospy.has_param("pid/mass"):
raise rospy.ROSException("UUV's mass was not provided")
if not rospy.has_param("pid/inertial"):
raise rospy.ROSException("UUV's inertial was not provided")
self.mass = rospy.get_param("pid/mass")
self.inertial = rospy.get_param("pid/inertial")
# update mass, moments of inertia
self.inertial_tensor = numpy.array([
[self.inertial['ixx'], self.inertial['ixy'], self.inertial['ixz']],
[self.inertial['ixy'], self.inertial['iyy'], self.inertial['iyz']],
[self.inertial['ixz'], self.inertial['iyz'], self.inertial['izz']]
])
self.mass_inertial_matrix = numpy.vstack(
(numpy.hstack((self.mass * numpy.identity(3), numpy.zeros(
(3, 3)))),
numpy.hstack((numpy.zeros((3, 3)), self.inertial_tensor))))
print self.mass_inertial_matrix
self.ready = True
def __init__(self, namespace=None, arm_name=None, compute_fk_for_all=False):
"""Class constructor"""
if None in [namespace, arm_name]:
ns = [item for item in rospy.get_namespace().split('/') if len(item) > 0]
if len(ns) != 2:
rospy.ROSException('The controller must be called inside the namespace the manipulator namespace')
namespace = ns[0]
arm_name = ns[1]
param_path = '/' + namespace + '/arms/' + arm_name
# Callbacks called in topic handlers
self._callbacks = {}
# Published topics
self._pubTopics = {}
# Subscribed topics
self._subTopics = {}
# Initializing the end-effector state
self._endeffector_state = EndEffectorState()
# Retrieve default configurations
self._default_configs = dict()
if rospy.has_param(param_path + '/default_configs'):
self._default_configs = rospy.get_param(param_path + '/default_configs')
else:
self._default_configs = None
base_link = None
tip_link = None
self._man_index = 0.0
assert rospy.has_param(param_path + '/base_link'), 'Base link name not available in the namespace %s' % namespace
assert rospy.has_param(param_path + '/tip_link'), 'Tip link name not available in the namespace %s' % namespace
# Retrieve the names of the base and tip links
base_link = rospy.get_param(param_path + '/base_link')
tip_link = rospy.get_param(param_path + '/tip_link')
# Initialize kinematics chain interface
KinChainInterface.__init__(self,
name=arm_name,
base=base_link,
ee_link=tip_link,
namespace=namespace,
arm_name=arm_name,
compute_fk_for_all=compute_fk_for_all)
self.set_joint_names(self._joint_names)
self._subTopics['joint_states'] = rospy.Subscriber(
self._namespace + 'joint_states',
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
def __init__(self, cloud, normal):
if not isinstance(cloud, PointCloud2):
rospy.ROSException('Argument 1 is not a sensor_msgs/PointCloud2')
pass
serial_cloud = to_cpp(cloud)
self._bounding_box = BoundingBoxWrapper(serial_cloud, normal)
pass
def __ensure_initialization(self):
if not self.__initialized:
try:
rhbplog.loginfo('Wait for knowledge base service: ' + self.__example_service_name)
rospy.wait_for_service(self.__example_service_name, timeout=self.__service_timeout)
self.__register_for_updates()
except rospy.ROSException as e:
raise rospy.ROSException("Timeout: Cannot reach service self.__example_service_name:" + str(e))
def __set_naked_pose(pose, naked_pose):
""" Return input pose placing position and rotation with those on naked_pose """
if not isinstance(naked_pose, geometry_msgs.Pose):
raise rospy.ROSException(
"Input parameter naked_pose is not a geometry_msgs.Pose!")
if isinstance(pose, geometry_msgs.PoseWithCovarianceStamped):
pose.pose.pose = naked_pose
elif isinstance(pose, geometry_msgs.PoseWithCovariance):
pose.pose = naked_pose
elif isinstance(pose, geometry_msgs.PoseStamped):
pose.pose = naked_pose
elif isinstance(pose, geometry_msgs.Pose):
pose = naked_pose
else:
raise rospy.ROSException(
"Input parameter pose is not any of geometry_msgs' poses!")
return pose
def __init__(self, *args, **kwargs):
super(ThrusterProportional, self).__init__(*args)
if 'gain' not in kwargs:
rospy.ROSException('Thruster gain not given')
self._gain = kwargs['gain']
rospy.loginfo('Thruster model:')
rospy.loginfo('\tGain=' + str(self._gain))
def add(self, a, b):
"""Add two std_mgs/Int64 messages
Return a std_msgs/Int64 instance.
Parameters
----------
- a: a std_msgs/Int64 instance.
- b: a std_msgs/Int64 instance.
"""
if not isinstance(a, Int64):
rospy.ROSException('Argument 1 is not a std_msgs/Int64')
if not isinstance(b, Int64):
rospy.ROSException('Argument 2 is not a std_msgs/Int64')
str_a = self._to_cpp(a)
str_b = self._to_cpp(b)
str_sum = self._add_two_ints.add(str_a, str_b)
return self._from_cpp(str_sum, Int64)
def to_pose2d(pose):
if isinstance(pose, geometry_msgs.PoseStamped):
p = pose.pose
elif isinstance(pose, geometry_msgs.Pose):
p = pose
else:
raise rospy.ROSException(
"Input parameter pose is not a valid geometry_msgs pose object")
return geometry_msgs.Pose2D(p.position.x, p.position.y, yaw(p))
def __build_tree(self, named_subscribers):
tree = {}
if not isinstance(named_subscribers, dict):
raise rospy.ROSException(
"Invalid dict->str tree passed to SubscriberTree")
for k, v in named_subscribers.items():
if isinstance(v, dict):
tree[k] = self.__build_tree(v)
elif isinstance(v, (str, int, float, list, bool)): # YAML types
tree[k] = AutoLogger(v,
callback=self.__callback,
pass_ref=self.__pass_ref)
else:
raise rospy.ROSException(
"Invalid dict->str tree passed to SubscriberTree")
return tree
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Last input update
self._last_update = rospy.get_time()
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Retrieve the publish rate
self._publish_rate = 1000
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._command_sub = rospy.Subscriber('gripper_control/command',
EndeffectorCommand,
self._on_gripper_command)
self._command = 'stop'
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = self._gripper.get_position_ratio(
self._gripper.closed_pos)
print 'ratio=', self._ratio_goal
self._kp = 100
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._controller_update_timer = rospy.Timer(
rospy.Duration(1.0 / self._publish_rate), self._update)
rospy.on_shutdown(self._on_shutdown)
def __init__(self):
ns = [
item for item in rospy.get_namespace().split('/') if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called in the manipulator namespace')
self._namespace = ns[0]
self._arm_name = ns[1]
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
# The arm interface loads the parameters from the URDF file and
# parameter server and initializes the KDL tree
self._arm_interface = ArmInterface()
self._base_link = self._arm_interface.base_link
self._tip_link = self._arm_interface.tip_link
self._tip_frame = PyKDL.Frame()
# KDL Solvers
self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain)
self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(
self._arm_interface.chain, self._arm_interface._fk_p_kdl,
self._ik_v_kdl, 100, 1e-6)
self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain,
PyKDL.Vector.Zero())
# Add a callback function to calculate the end effector's state by each
# update of the joint state
self._arm_interface.add_callback('joint_states',
self.on_update_endeffector_state)
# Publish the current manipulability index at each update of the joint
# states
# self._arm_interface.add_callback('joint_states',
# self.publish_man_index)
# Publish the end effector state
self._endeffector_state_pub = rospy.Publisher('endpoint_state',
EndPointState,
queue_size=1)
# Publish the manipulability index
self._man_index_pub = rospy.Publisher('man_index',
Float64,
queue_size=1)
self._services = dict()
# Provide the service to calculate the inverse kinematics using the KDL solver
self._services['ik'] = rospy.Service('ik_solver', SolveIK,
self.solve_ik)
def __init__(self, backend=None, backend_kwargs=None, service_name=default_service_name):
self._service_name = service_name
if backend is None:
backend = rospy.get_param('~backend', "default")
if backend not in DETECTION_REGISTRY:
rospy.logerr(
"Backend '{}' not in registry see README.md file and add it as a backend! Available backends: {}".format(
backend, DETECTION_REGISTRY.available_backends()))
sys.exit(1)
rospy.loginfo("Configuring detection service for backend '{}'".format(backend))
# Parse passed parameters (fail if required missing, override if optional present)
required_args, optional_args = DETECTION_REGISTRY.get_arguments(backend)
assigned_parameters = ["~backend"]
if backend_kwargs is None:
# Fill in required backend arguments from the private ros parameter server
backend_kwargs = {}
for arg_name in required_args:
p_arg = "~" + arg_name
if rospy.has_param(p_arg):
assigned_parameters.append(p_arg)
backend_kwargs[arg_name] = rospy.get_param(p_arg)
# Replace optional parameters if they exist
for arg_name in optional_args:
p_arg = "~" + arg_name
if rospy.has_param(p_arg):
assigned_parameters.append(p_arg)
backend_kwargs[arg_name] = rospy.get_param(p_arg)
# Ensure the required args exist
_missing_args = [a for a in required_args if a not in backend_kwargs]
if len(_missing_args) > 0:
raise rospy.ROSException("Cannot initialise without the '{}' parameter(s) for the '{}' backend".format(
", ".join(_missing_args), backend
))
self.server_args = backend_kwargs
self.server_args['service_name'] = service_name
self.backend_name = backend
self._assigned_parameters = assigned_parameters
# Register callback to delete parameters from ROS param server on shutdown
rospy.on_shutdown(self.on_shutdown)
# Get the backend
self.server = DETECTION_REGISTRY[self.backend_name]
notice = getattr(self.server, "citation_notice", None)
if notice:
if callable(notice):
notice = notice()
if isinstance(notice, str):
for msg in notice.split("\n"):
rospy.loginfo("\033[93m{}\033[0m: {}".format(self.backend_name, msg))
def setNegative(self, negative):
'''Set whether the regular (or inverted) conditions for points filtering should apply
Parameters
----------
- negative : false = normal filter behavior (default), true = inverted behavior
'''
if not isinstance(negative, bool):
rospy.ROSException('negative is not bool')
_neg = self._to_cpp(Bool(negative))
self._passthrough.setNegative(_neg)
def test_worldSDF_service_wait_for_service_fail(self,
mckd_wait_for_service):
mckd_wait_for_service.side_effect = rospy.ROSException(
'Mocked ROSException')
# call the service
response = self.client.get('/simulation/0/sdf_world')
# check the status code
self.assertEqual(response.status_code, 500)
def removePointCloud(self, cloud_id, viewport=0): '''Remove Point Cloud from Screen Returns the operation success Parameters ---------- - cloud_id : the point cloud object id (taken from addPointCloud) - viewport : view port from where the Point Cloud should be removed (default: all) ''' if not isinstance( cloud_id, str ): rospy.ROSException( 'cloud_id is not str' ) if not isinstance(viewport, int): rospy.ROSException( 'viewport is not int' ) _cloud_id = self._to_cpp( String(cloud_id) ) _viewport = self._to_cpp( Int32(viewport) ) ret = self._viewer.removePointCloud(_cloud_id, _viewport) _ret = self._from_cpp( ret, Bool ) return _ret.data
def ros_msg_from_string(msg_cls):
if msg_cls in ["genpy.Time", "genpy.Duration"]:
cls_type = rospy.rostime.Time if ".Time" in msg_cls else rospy.rostime.Duration
else:
cls_type = roslib.message.get_message_class(msg_cls)
if not cls_type:
raise rospy.ROSException(
"Cannot load message class for [{}]. Please ensure the relevant package "
"is installed on your system.".format(msg_cls))
return cls_type
def test_experiment_world_sdf_wrong_xml_2(self, mocked_rospy):
experiment_id = '123456'
MockServiceResponse.sdf_dump = "<sdf/>"
mocked_rospy.wait_for_service.side_effect = rospy.ROSException(
'Mocked ROSException')
body = {'context_id': 'fake'}
response = self.client.post('/experiment/' + experiment_id +
'/sdf_world',
data=json.dumps(body))
self.assertEqual(response.status_code, 500)
def addPointCloud(self, cloud, viewport=0): '''Add Point Cloud to Screen Returns a cloud_id Parameters ---------- - cloud : the input cloud data set - viewport : the view port where the Point Cloud should be added (default: all) ''' if not isinstance( cloud, PointCloud2 ): rospy.ROSException( 'cloud is not a PointCloud2' ) if not isinstance(viewport, int): rospy.ROSException( 'viewport is not int' ) _cloud = self._to_cpp( cloud ) _viewport = self._to_cpp( Int32(viewport) ) _ret = self._viewer.addPointCloud( _cloud, _viewport ) ret = self._from_cpp(_ret, String) return ret.data
def setLeafSize(self, lx, ly, lz):
'''Set the voxel grid leaf size
Parameters
----------
- lx : leaf size for X
- ly : leaf size for Y
- lz : leaf size for Z
'''
if not isinstance(lx, float):
rospy.ROSException('lx is not float')
if not isinstance(ly, float):
rospy.ROSException('ly is not float')
if not isinstance(lz, float):
rospy.ROSException('lz is not float')
_lx = self._to_cpp(Float64(lx))
_ly = self._to_cpp(Float64(ly))
_lz = self._to_cpp(Float64(lz))
self._voxel_grid.setLeafSize(_lx, _ly, _lz)
def savePCDFile(self, filename, cloud): '''Save PCD file Returns the operation success (True, False) Parameters ---------- - filename: filename to save the pcd file - cloud: cloud to save ''' if not isinstance( filename, str ): rospy.ROSException( 'filename is not str' ) if not isinstance( cloud, PointCloud2 ): rospy.ROSException( 'cloud is not a PointCloud2' ) str_cloud = self._to_cpp( cloud ) str_filename = self._to_cpp( String(filename) ) s_ret = self._pcd_io.savePCDFile( str_filename, str_cloud ) ret = self._from_cpp( s_ret, Bool ) return ret.data
def get_plan_to_joint_state(self, goal_joints):
num_goal_joints = len(goal_joints)
num_robot_joints = len(self.group.get_current_joint_values())
if num_goal_joints != num_robot_joints:
raise rospy.ROSException("Gave a joint command with {} joint values, expected {} joint values.".format(num_goal_joints, num_robot_joints))
self.group.set_start_state(self.robot.get_current_state())
plan = self.group.plan(goal_joints)
return plan
def setTransformationEpsilon(self, epsilon): '''Set the transformation epsilon (maximum allowable translation squared difference between two consecutive transformations) Parameters ---------- - epsilon : the transformation epsilon in order for an optimization to be considered as having converged to the final solution ''' if not isinstance(epsilon, float): rospy.ROSException( 'epsilon is not float' ) s_epsilon = self._to_cpp( Float64(epsilon) ) self._icp.setTransformationEpsilon(s_epsilon)
def __init__(self):
try:
self.device = ArduinoUsbDevice(idVendor=0x16c0, idProduct=0x05df)
except:
raise rospy.ROSException("No DigiUSB Device Found")
rospy.init_node('temperature_sensor_publisher')
self.pub = rospy.Publisher('temp', Temperature, queue_size=50)
self.rate = 1.0
def setMaximumIterations(self, nr_iterations): '''Set the maximum number of iterations the internal optimization should run for Parameters ---------- - nr_iterations : the maximum number of iterations the internal optimization should run for ''' if not isinstance(nr_iterations, int): rospy.ROSException( 'nr_iterations is not int' ) s_iters = self._to_cpp( Int32(nr_iterations) ) self._icp.setMaximumIterations(s_iters)
def __get_msg_tree(self, data_tree):
if not isinstance(data_tree, dict):
raise rospy.ROSException(
"Invalid dict->str tree passed to get msg tree in SubscriberTree"
)
msg_tree = {
k: v.data if not isinstance(v, dict) else self.__get_msg_tree(v)
for k, v in data_tree.items()
}
return msg_tree
def setEuclideanFitnessEpsilon(self, epsilon): '''Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop Parameters ---------- - epsilon : the maximum allowed distance error before the algorithm will be considered to have converged ''' if not isinstance(epsilon, float): rospy.ROSException( 'epsilon is not float' ) s_epsilon = self._to_cpp( Float64(epsilon) ) self._icp.setEuclideanFitnessEpsilon(s_epsilon)
def __init__(self, *args, **kwargs):
"""Class constructor."""
super(ThrusterCustom, self).__init__(*args)
if 'input' not in kwargs or 'output' not in kwargs:
rospy.ROSException('Thruster input/output sample points not given')
# Vector of sample values for each angular velocity
self._input = kwargs['input']
# Vector of sample values for each thrust force relative to the input
self._output = kwargs['output']
