def set_link_wrench(self, force, torque, duration, starting_time):
ns = self.get_namespace().replace('/', '')
body_name = '%s/base_link' % ns
req = ApplyLinkWrench.Request()
self._body_force = np.array([float(self._body_force[i]) + force[i] for i in range(3)])
self._body_torque = np.array([float(self._body_torque[i]) + torque[i] for i in range(3)])
self._body_wrench_msg = WrenchStamped()
self._body_wrench_msg.header.stamp = self.get_clock().now().to_msg()
self._body_wrench_msg.header.frame_id = 'world'
self._body_wrench_msg.wrench.force = Vector3(x=self._body_force[0],
y=self._body_force[1],
z=self._body_force[2])
self._body_wrench_msg.wrench.torque = Vector3(x=self._body_torque[0],
y=self._body_torque[1],
z=self._body_torque[2])
(secs, nsecs) = float_sec_to_int_sec_nano(starting_time)
(d_secs, d_nsecs) = float_sec_to_int_sec_nano(duration)
req.link_name = body_name
req.reference_frame = 'world'
req.reference_point = Point(x=0., y=0., z=0.)
req.wrench = self._body_wrench_msg.wrench
req.start_time = rclpy.time.Time(seconds=secs, nanoseconds=nsecs).to_msg()
req.duration = rclpy.time.Duration(seconds=d_secs, nanoseconds=d_nsecs).to_msg()
future = self._service_cb['wrench'].call_async(req)
self.wait_for_service_completion(
future,
'Link wrench perturbation applied!, body_name=%s, t=%.2f s' % (body_name, time_in_float_sec(self.get_clock().now())),
'Failed to apply link wrench!, body_name=%s, t=%.2f s' % (body_name, time_in_float_sec(self.get_clock().now())))
def get_path_message(self):
path_msg = Path()
try:
path_msg.header.stamp = self.node.get_clock().now().to_msg()
set_timestamps = True
except:
set_timestamps = False
self._logger.warning('ROS node was not initialized, no timestamp '
'can be assigned to path message')
path_msg.header.frame_id = 'world'
path_msg.poses = list()
for point in self._local_planner.points:
pose = PoseStamped()
if set_timestamps:
(secs, nsecs) = float_sec_to_int_sec_nano(point.t)
pose.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs)
pose.pose.position = Vector3(x=point.p[0],
y=point.p[1],
z=point.p[2])
pose.pose.orientation = Quaternion(x=point.q[0],
y=point.q[1],
z=point.q[2],
w=point.q[3])
path_msg.poses.append(pose)
def to_message(self):
"""Convert current data to a trajectory point message.
> *Returns*
Trajectory point message as `uuv_control_msgs/TrajectoryPoint`
"""
p_msg = TrajectoryPointMsg()
# FIXME Sometimes the time t stored is NaN
(secs, nsecs) = float_sec_to_int_sec_nano(self.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs).to_msg()
p_msg.pose.position = geometry_msgs.Point(x=self.p[0],
y=self.p[1],
z=self.p[2])
p_msg.pose.orientation = geometry_msgs.Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
p_msg.velocity.linear = geometry_msgs.Vector3(x=self.v[0],
y=self.v[1],
z=self.v[2])
p_msg.velocity.angular = geometry_msgs.Vector3(x=self.w[0],
y=self.w[1],
z=self.w[2])
p_msg.acceleration.linear = geometry_msgs.Vector3(x=self.a[0],
y=self.a[1],
z=self.a[2])
p_msg.acceleration.angular = geometry_msgs.Vector3(x=self.alpha[0],
y=self.alpha[1],
z=self.alpha[2])
return p_msg
def main():
rclpy.init()
sim_time_param = is_sim_time()
# Compared to ROS 1 version, the node name was changed
node = rclpy.create_node(
'start_helical_trajectory',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Starting the helical trajectory creator')
# Ensure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
rclpy.spin_once(node)
# If no start time is provided: start *now*.
start_time = time_in_float_sec(node.get_clock().now())
start_now = False
if node.has_parameter('start_time'):
start_time = node.get_parameter('start_time').value
if start_time < 0.0:
node.get_logger().warn('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = True
param_labels = [
'radius', 'center', 'n_points', 'heading_offset', 'duration',
'n_turns', 'delta_z', 'max_forward_speed'
]
params = dict()
for label in param_labels:
if not node.has_parameter(label):
node.get_logger().error(
'{} must be provided for the trajectory generation!'.format(
label))
sys.exit(-1)
params[label] = node.get_parameter(label).value
if len(params['center']) != 3:
raise RuntimeError(
'Center of circle must have 3 components (x, y, z):' +
str(params['center']))
if params['n_points'] <= 2:
raise RuntimeError('Number of points must be at least 2' +
str(params['n_points']))
if params['max_forward_speed'] <= 0:
raise RuntimeError('Velocity limit must be positive' +
str(params['max_forward_speed']))
srv_name = 'start_helical_trajectory'
traj_gen = node.create_client(InitHelicalTrajectory,
'start_helical_trajectory')
if not traj_gen.wait_for_service(timeout_sec=20):
raise RuntimeError('Service %s not available! Closing node...' %
(traj_gen.srv_name))
node.get_logger().info(
'Generating trajectory that starts at t={} s'.format(start_time))
# Convert the time value
(sec, nsec) = float_sec_to_int_sec_nano(start_time)
req = InitHelicalTrajectory.Request()
req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
req.start_now = start_now
req.radius = float(params['radius'])
req.center = Point(x=float(params['center'][0]),
y=float(params['center'][1]),
z=float(params['center'][2]))
req.is_clockwise = False
req.angle_offset = 0.0
req.n_points = params['n_points']
req.heading_offset = float(params['heading_offset'] * pi / 180)
req.max_forward_speed = float(params['max_forward_speed'])
req.duration = float(params['duration'])
# As partial turns can be given, it is a float
req.n_turns = float(params['n_turns'])
req.delta_z = float(params['delta_z'])
future = traj_gen.call_async(req)
rclpy.spin_until_future_complete(node, future)
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + str(e))
else:
node.get_logger().info('Trajectory successfully generated!')
node.destroy_node()
def gen_trajectory_message(self):
if self._points is None:
return None
msg = uuv_control_msgs.Trajectory()
try:
msg.header.stamp = self.node.get_clock().now().to_msg()
set_timestamps = True
except:
set_timestamps = False
self._logger.warning('ROS node was not initialized, no timestamp '
'can be assigned to trajectory message')
msg.header.frame_id = 'world'
#TODO Simplify following
if not self.is_using_waypoints:
for p in self._points:
p_msg = uuv_control_msgs.TrajectoryPoint()
if set_timestamps:
(secs, nsecs) = float_sec_to_int_sec_nano(p.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs)
p_msg.pose.position = Vector3(x=p.p[0], y=p.p[1], z=p.p[2])
p_msg.pose.orientation = Quaternion(x=p.q[0],
y=p.q[1],
z=p.q[2],
w=p.q[3])
p_msg.velocity.linear = Vector3(x=p.v[0], y=p.v[1], z=p.v[2])
p_msg.velocity.angular = Vector3(x=p.w[0], y=p.w[1], z=p.w[2])
p_msg.acceleration.linear = Vector3(x=p.a[0],
y=p.a[1],
z=p.a[2])
p_msg.acceleration.angular = Vector3(x=p.alpha[0],
y=p.alpha[1],
z=p.alpha[2])
msg.points.append(p_msg)
else:
dt = 0.05 * self._wp_interp.get_max_time()
for ti in np.arange(0, self._wp_interp.get_max_time(), dt):
pnt = self._wp_interp.interpolate(ti)
p_msg = uuv_control_msgs.TrajectoryPoint()
if set_timestamps:
(secs, nsecs) = float_sec_to_int_sec_nano(pnt.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs)
p_msg.pose.position = Vector3(x=pnt.p[0],
y=pnt.p[1],
z=pnt.p[2])
p_msg.pose.orientation = Quaternion(x=pnt.q[0],
y=pnt.q[1],
z=pnt.q[2],
w=pnt.q[3])
p_msg.velocity.linear = Vector3(x=pnt.v[0],
y=pnt.v[1],
z=pnt.v[2])
p_msg.velocity.angular = Vector3(x=pnt.w[0],
y=pnt.w[1],
z=pnt.w[2])
p_msg.acceleration.linear = Vector3(x=pnt.a[0],
y=pnt.a[1],
z=pnt.a[2])
p_msg.acceleration.angular = Vector3(x=pnt.alpha[0],
y=pnt.alpha[1],
z=pnt.alpha[2])
msg.points.append(p_msg)
return msg
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'send_waypoint_file',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Send a waypoint file, namespace=%s' %
node.get_namespace())
if node.has_parameter('filename'):
filename = node.get_parameter(
'filename').get_parameter_value().string_value
else:
raise RuntimeError('No filename found')
# Important...ensure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
rclpy.spin_once(node)
# If no start time is provided: start *now*.
start_time = time_in_float_sec(node.get_clock().now())
start_now = True
if node.has_parameter('start_time'):
start_time = node.get_parameter('start_time').value
if start_time < 0.0:
node.get_logger().warn('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = False
node.get_logger().info('Start time=%.2f s' % start_time)
interpolator = node.get_parameter_or(
'interpolator', 'lipb').get_parameter_value().string_value
srv_name = 'init_waypoints_from_file'
init_wp = node.create_client(InitWaypointsFromFile, srv_name)
ready = init_wp.wait_for_service(timeout_sec=30)
if not ready:
raise RuntimeError('Service not available! Closing node...')
(sec, nsec) = float_sec_to_int_sec_nano(start_time)
req = InitWaypointsFromFile.Request()
req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
req.start_now = start_now
req.filename = String(data=filename)
req.interpolator = String(data=interpolator)
future = init_wp.call_async(req)
rclpy.spin_until_future_complete(node, future)
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + repr(e))
else:
node.get_logger().info('Waypoints file successfully received, '
'filename=%s' % filename)
node.destroy_node()
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_link_wrench',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Apply programmed perturbation to vehicle ' +
node.get_namespace())
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = float(node.get_parameter('starting_time').value)
node.get_logger().info('Starting time in = {} s'.format(starting_time))
duration = 0.0
if node.has_parameter('duration'):
duration = float(node.get_parameter('duration').value)
# Compare to eps ?
if duration == 0.0:
node.get_logger().info('Duration not set, leaving node...')
sys.exit(-1)
node.get_logger().info('Duration [s]=' +
('Inf.' if duration < 0 else str(duration)))
force = [0.0, 0.0, 0.0]
if node.has_parameter('force'):
force = node.get_parameter('force').value
if len(force) != 3:
raise RuntimeError('Invalid force vector')
# Ensure type is float
force = [float(elem) for elem in force]
node.get_logger().info('Force [N]=' + str(force))
torque = [0.0, 0.0, 0.0]
if node.has_parameter('torque'):
torque = node.get_parameter('torque').value
if len(torque) != 3:
raise RuntimeError('Invalid torque vector')
# Ensure type is float
torque = [float(elem) for elem in torque]
node.get_logger().info('Torque [N]=' + str(torque))
gazebo_ns = 'gazebo'
if node.has_parameter('gazebo_ns'):
gazebo_ns = node.get_parameter('gazebo_ns').value
if not gazebo_ns.startswith('/'):
gazebo_ns = '/' + gazebo_ns
service_name = '{}/apply_link_wrench'.format(gazebo_ns)
try:
apply_wrench = node.create_client(ApplyLinkWrench, service_name)
except Exception as e:
node.get_logger().error('Creation of service ' + service_name +
' failed, error=' + str(e))
sys.exit(-1)
try:
ready = apply_wrench.wait_for_service(timeout_sec=10)
if not ready:
raise RuntimeError('')
except:
node.get_logger().error('Service ' + service_name +
' not available! Closing node...')
sys.exit(-1)
ns = node.get_namespace().replace('/', '')
body_name = '%s/base_link' % ns
FREQ = 100
rate = node.create_rate(FREQ)
thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
# Just a guard for really short timeouts
if 1.0 / FREQ < starting_time:
rate.sleep()
(d_secs, d_nsecs) = float_sec_to_int_sec_nano(duration)
apw = ApplyLinkWrench.Request()
apw.link_name = body_name
apw.reference_frame = 'world'
apw.reference_point = Point(x=0.0, y=0.0, z=0.0)
apw.wrench = Wrench()
apw.wrench.force = Vector3(x=force[0], y=force[1], z=force[2])
apw.wrench.torque = Vector3(x=torque[0], y=torque[1], z=torque[2])
apw.start_time = node.get_clock().now().to_msg()
apw.duration = rclpy.time.Duration(seconds=d_secs,
nanoseconds=d_nsecs).to_msg()
future = apply_wrench.call_async(apw)
# NB : spining is done from another thread
while rclpy.ok():
if future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().error(
'Could not apply link wrench, exception thrown: %r' %
(e, ))
else:
if response.success:
message = '\nLink wrench perturbation applied!'
message += '\n\tFrame: ' + body_name
message += '\n\tDuration [s]: ' + str(duration)
message += '\n\tForce [N]: ' + str(force)
message += '\n\tTorque [Nm]: ' + str(torque)
node.get_logger().info(message)
else:
message = 'Could not apply link wrench'
message += '\n\tError is: %s' % (response.status_message)
node.get_logger().error(message)
finally:
break
node.destroy_node()
