def main():
print('Starting Non-model-based Sliding Mode Controller')
rclpy.init()
try:
sim_time_param = is_sim_time()
tf_world_ned_to_enu = get_world_ned_to_enu(sim_time_param)
node = ROV_NMB_SMController('rov_nmb_sm_controller',
world_ned_to_enu=tf_world_ned_to_enu,
parameter_overrides=[sim_time_param])
executor = rclpy.executors.MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
# rclpy.spin(node)
except KeyboardInterrupt:
pass
except Exception as e:
print('Caught exception: ' + repr(e))
traceback.print_exc()
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
print('Starting position_control.py')
rclpy.init()
#try:
if 1:
sim_time_param = is_sim_time()
node = PositionControllerNode('position_control',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node('unpause_simulation',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
#Default sim_time to True
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
# if not rclpy.ok():
# rospy.ROSException('ROS master is not running!')
timeout = 0.0
if node.has_parameter('timeout'):
timeout = node.get_parameter('timeout').get_parameter_value().double_value
if timeout <= 0:
raise RuntimeError('Unpause time must be a positive floating point value')
print('Unpause simulation - Time = {} s'.format(timeout))
# start_time = time.time()
# while time.time() - start_time < timeout:
# time.sleep(0.1)
if(timeout > 0):
time.sleep(timeout)
#start_time = time.time()
try:
# Handle for retrieving model properties
unpause = node.create_client(Empty, '/gazebo/unpause_physics')
unpause.wait_for_service(timeout_sec=100)
if(not ready):
raise rclpy.exceptions.InvalidServiceNameException('service is unavailable')
except rclpy.exceptions.InvalidServiceNameException:
print('/gazebo/unpause_physics service is unavailable')
sys.exit()
node.get_logger().info(
'The Gazebo "unpause_physics" service was available {} s after the timeout'.format(timeout))#time.time() - start_time))
req = Empty.Request()
future = unpause.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
prop = future.result()
if prop.succes:
print('Simulation unpaused...')
else
node.get_logger().error('Failed to unpaused the simulation')
def main():
print('Start publishing vehicle footprints to RViz')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = FootprintsPublisher('publish_footprints', parameter_overrides=[sim_time_param])
rclpy.spin(node)
except rclpy.exceptions.ROSInterruptException:
print('caught exception')
print('exiting')
def main():
print('Generate RViz footprint and markers for 2D visualization')
#TODO Add rclpy instruction
try:
sim_time_param = is_sim_time()
world_pub = WorldPublisher('publish_world_models', parameter_overrides=[sim_time_param])
node = VehicleFootprint('generate_vehicle_footprint')
rclpy.spin(node)
except rclpy.exceptions.ROSInterruptException as e:
print('Caught exception: ' + str(e))
print('Exiting')
def main():
print('Start publishing vehicle footprints to RViz')
rclpy.init()
try:
sim_time_param = is_sim_time()
world_pub = WorldPublisher('publish_world_models',
parameter_overrides=[sim_time_param])
rclpy.spin(world_pub)
except Exception as e:
print('Caught exception: ' + str(e))
print('Exiting')
def main():
print('Starting VelocityControl.py')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = VelocityControllerNode('velocity_control', parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + str(e))
finally:
rclpy.shutdown()
print('Exiting')
def main():
rclpy.init()
try:
sim_time_param = is_sim_time()
node = ROV_NLPIDController('rov_nl_pid_controller',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('exiting')
def main(args=None):
# Wait for 5 seconds, so the instructions are the last thing to print in terminal
time.sleep(5)
# Start the node
name = os.path.splitext(os.path.basename(__file__))[0]
rclpy.init()
sim_time_param = is_sim_time()
teleop = KeyBoardVehicleTeleop(name, parameter_overrides=[sim_time_param])
teleop.get_logger().info('Starting [%s] node' % name)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, teleop.settings)
rclpy.spin(teleop)
teleop.get_logger().info('Shutting down [%s] node' % name)
rclpy.shutdown()
def main():
rclpy.init()
try:
sim_time_param = is_sim_time()
node = ThrusterAllocatorNode('thruster_allocator',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('ThrusterAllocatorNode::Exception ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('Leaving ThrusterAllocatorNode')
def main():
print('starting acceleration_control.py')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = AccelerationControllerNode('acceleration_control', parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception:' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
print('Starting AUV trajectory tracker')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = AUVGeometricTrackingController(
'auv_geometric_tracking_controller',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('caught exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
def main(args=None):
# Start the node
node_name = os.path.splitext(os.path.basename(__file__))[0]
rclpy.init()
try:
sim_time_param = is_sim_time()
teleop = VehicleTeleop(node_name, parameter_overrides=[sim_time_param])
teleop.get_logger().info('Starting [%s] node' % node_name)
rclpy.spin(teleop)
except Exception as e:
print('Caught exception: ' + str(e))
teleop.get_logger().info('Shutting down [%s] node' % node_name)
rclpy.shutdown()
def main():
print('Starting Model-based Sliding Mode Controller')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = ROV_MB_SMController('rov_mb_sm_controller',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_simulation_timer',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
timeout = 0.0
if node.has_parameter('timeout'):
timeout = node.get_parameter(
'timeout').get_parameter_value().double_value
if timeout <= 0:
raise RuntimeError(
'Termination time must be a positive floating point value (X.Y)'
)
node.get_logger().info(
'Starting simulation timer - Timeout = {} s'.format(timeout))
# Note that node's clock will be initialized from the /clock topic during the spin in sim tim mode
# Behaviour of Rate changed in ROS 2. To wake it up, it needs to be triggered from a separate
# thread. Maybe a rclpy.spin_once + time.sleep() would be enough
FREQ = 100
rate = node.create_rate(
FREQ
) #, rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.STEADY_TIME))
thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < timeout:
#rclpy.spin_once(node)
#Just a guard for really short timeouts
if 1.0 / FREQ < timeout:
rate.sleep()
node.get_logger().info('Simulation timeout - Killing simulation...')
# destroy_node() prevents a further warning on exit
node.destroy_node()
rclpy.shutdown()
thread.join()
def main(args=None):
print('starting FinnedUUVControllerNode.py')
rclpy.init(args=args)
try:
sim_time_param = is_sim_time()
node = FinnedUUVControllerNode(parameter_overrides=[sim_time_param])
rclpy.spin(node)
except rclpy.exceptions.ROSInterruptException as excep:
print('Caught ROSInterruptException exception: ' + str(excep))
except Exception as e:
print('Caught exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
print('Starting orientation_control.py')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = OrientationControllerNode('orientation_control',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + str(e))
finally:
node.pub_cmd_vel.publish(Twist())
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
print('Starting PD controller with compensation of restoring forces')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = ROV_PD_GComp_Controller('rov_pd_grav_compensation_controller',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
print('Starting Modelbased Feedback Linearization Controller')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = ROV_MBFLController('rov_mb_fl_controller',
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except rclpy.exceptions.ROSInterruptException as excep:
print('Caught ROSInterruptException exception: ' + str(excep))
except Exception as e:
print('Caught exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
rclpy.init()
try:
sim_time_param = is_sim_time()
tf_world_ned_to_enu = get_world_ned_to_enu(sim_time_param)
node = ROV_NLPIDController('rov_nl_pid_controller',
world_ned_to_enu=tf_world_ned_to_enu,
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + repr(e))
traceback.print_exc()
finally:
if rclpy.ok():
rclpy.shutdown()
print('exiting')
def main():
print('Starting Model-based Sliding Mode Controller')
rclpy.init()
try:
sim_time_param = is_sim_time()
tf_world_ned_to_enu = get_world_ned_to_enu(sim_time_param)
node = ROV_MB_SMController('rov_mb_sm_controller',
parameter_overrides=[sim_time_param],
world_ned_to_enu=tf_world_ned_to_enu)
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + repr(e))
traceback.print_exc()
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
print('Starting disturbance manager')
rclpy.init()
try:
sim_time_param = is_sim_time()
node = DisturbanceManager('disturbance_manager', parameter_overrides=[sim_time_param])
rclpy.spin(node)
except rclpy.exceptions.ROSInterruptException as rosInter:
print('Caught ROSInterruptException exception' + str(rosInter))
except Exception as e:
print('Caught exception: '+str(e))
# finally:
# if rclpy.ok():
# rclpy.shutdown()
# node.destroy_node()
# node.thread.join()
print('Exiting')
def main():
print('Starting AUV trajectory tracker')
rclpy.init()
try:
sim_time_param = is_sim_time()
tf_world_ned_to_enu = get_world_ned_to_enu(sim_time_param)
node = AUVGeometricTrackingController(
'auv_geometric_tracking_controller',
world_ned_to_enu=tf_world_ned_to_enu,
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('caught exception: ' + repr(e))
traceback.print_exc()
finally:
if rclpy.ok():
rclpy.shutdown()
def main():
print('Starting PD controller with compensation of restoring forces')
rclpy.init()
try:
sim_time_param = is_sim_time()
tf_world_ned_to_enu = get_world_ned_to_enu(sim_time_param)
node = ROV_PD_GComp_Controller('rov_pd_grav_compensation_controller',
world_ned_to_enu=tf_world_ned_to_enu,
parameter_overrides=[sim_time_param])
rclpy.spin(node)
except Exception as e:
print('Caught exception: ' + repr(e))
traceback.print_exc()
finally:
if rclpy.ok():
rclpy.shutdown()
print('Exiting')
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node('unpause_simulation',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
timeout = 0.0
if node.has_parameter('timeout'):
timeout = node.get_parameter('timeout').get_parameter_value().double_value
if timeout <= 0:
raise RuntimeError('Unpause time must be a positive floating point value')
print('Unpause simulation - Time = {} s'.format(timeout))
if(timeout > 0):
time.sleep(timeout)
# Handle for retrieving model properties
unpause = node.create_client(Empty, '/gazebo/unpause_physics')
unpause.wait_for_service(timeout_sec=100)
if(not ready):
raise Exception('Service %s is unavailable' % unpause.srv_name)
sys.exit()
node.get_logger().info(
'The Gazebo "unpause_physics" service was available {} s after the timeout'.format(timeout))
req = Empty.Request()
future = unpause.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
prop = future.result()
if prop.succes:
print('Simulation unpaused...')
else
node.get_logger().error('Failed to unpaused the simulation')
def main():
rclpy.init()
node = None
try:
sim_time_param = is_sim_time()
node = DisturbanceManager('disturbance_manager', parameter_overrides=[sim_time_param])
node.run()
except rclpy.exceptions.ROSInterruptException as rosInter:
print('Caught ROSInterruptException exception' + str(rosInter))
except Exception as e:
print('Caught exception: '+ repr(e))
print(traceback.print_exc())
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
# node.destroy_node()
# node.thread.join()
print('Exiting')
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_thrusters_states',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
node.get_logger().info('Set the state of thrusters for vehicle, namespace=' + node.get_namespace())
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = node.get_parameter('starting_time').value
node.get_logger().info('Starting time={} s'.format(starting_time))
duration = -1.0
if node.has_parameter('duration'):
duration = node.get_parameter('duration').value
if duration <= 0.0:
raise RuntimeError('Duration not set or negative, leaving node...')
node.get_logger().info('Duration [s]=', ('Inf.' if duration < 0 else str(duration)))
if node.has_parameter('is_on'):
is_on = bool(node.get_parameter('is_on').get_parameter_value().bool_value)
else:
raise RuntimeError('is_on state flag not provided')
thruster_id = -1
if node.has_parameter('thruster_id'):
if node.get_parameter('thruster_id').type_ == rclpy.Parameter.Type.INTEGER:
thruster_id = node.get_parameter('thruster_id').get_parameter_value().integer_value()
else:
raise RuntimeError('Thruster ID not given')
if thruster_id < 0:
raise RuntimeError('Invalid thruster ID')
node.get_logger().info('Setting state of thruster #{} as {}'.format(thruster_id, 'ON' if is_on else 'OFF'))
vehicle_name = node.get_namespace().replace('/', '')
srv_name = build_service_name(vehicle_name, thruster_id, 'set_thruster_state')
try:
set_state = node.create_client(SetThrusterState, srv_name)
except Exception as e:
raise RuntimeError('Service call failed, error=' + str(e))
if not set_state.wait_for_service(timeout_sec=2):
raise RuntimeError('Service %s not available! Closing node...' %(srv_name))
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()
# rate = node.create_rate(100)
# while time_in_float_sec(node.get_clock().now()) < starting_time:
# rate.sleep()
req = SetThrusterState.Request()
req.on = is_on
success = set_state.call(req)
future = set_state.call_async(req)
#NB : spining is done from another thread
while not future.done():
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('Time={} s'.format(time_in_float_sec(node.get_clock().now().get_time())))
node.get_logger().info('Current state of thruster #{}={}'.format(thruster_id, 'ON' if is_on else 'OFF'))
# if success:
# print('Time={} s'.format(time_in_float_sec(node.get_clock().now().get_time())))
# print('Current state of thruster #{}={}'.format(thruster_id, 'ON' if is_on else 'OFF'))
if duration > 0:
while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
if 1.0 / FREQ < starting_time + duration:
rate.sleep()
# rate = node.create_rate(100)
# while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
# rate.sleep()
req.on = not is_on
success = set_state.call(req)
while not future.done():
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('Time={} s'.format(time_in_float_sec(node.get_clock().now().get_time())))
node.get_logger().info('Returning to previous state of thruster #{}={}'.format(thruster_id, 'ON' if not is_on else 'OFF'))
# if success:
# print('Time={} s'.format(time_in_float_sec(node.get_clock().now().get_time())))
# print('Returning to previous state of thruster #{}={}'.format(thruster_id, 'ON' if not is_on else 'OFF'))
node.get_logger().info('Leaving node...')
node.destroy_node()
rclpy.shutdown()
thread.join()
def launch_setup(context, *args, **kwargs):
# Perform substitutions
debug = Lc('debug').perform(context)
namespace = Lc('namespace').perform(context)
x = Lc('x').perform(context)
y = Lc('y').perform(context)
z = Lc('z').perform(context)
roll = Lc('roll').perform(context)
pitch = Lc('pitch').perform(context)
yaw = Lc('yaw').perform(context)
# use_sim_time = Lc('use_sim_time').perform(context)
use_world_ned = Lc('use_ned_frame').perform(context)
# Request sim time value to the global node
res = is_sim_time(return_param=False, use_subprocess=True)
# Xacro
#xacro_file = PathJoinSubstitution(get_package_share_directory('uuv_descriptions'),'robots','rexrov_')
xacro_file = os.path.join(
get_package_share_directory('uuv_descriptions'), 'robots',
'rexrov_' + (Lc('mode')).perform(context) + '.xacro')
# Build the directories, check for existence
path = os.path.join(
get_package_share_directory('uuv_descriptions'),
'robots',
'generated',
namespace,
)
if not pathlib.Path(path).exists():
try:
# Create directory if required and sub-directory
os.makedirs(path)
except OSError:
print("Creation of the directory %s failed" % path)
output = os.path.join(path, 'robot_description')
if not pathlib.Path(xacro_file).exists():
exc = 'Launch file ' + xacro_file + ' does not exist'
raise Exception(exc)
mapping = {}
if to_bool(use_world_ned):
mappings = {
'debug': debug,
'namespace': namespace,
'inertial_reference_frame': 'world_ned'
}
else:
mappings = {
'debug': debug,
'namespace': namespace,
'inertial_reference_frame': 'world'
}
doc = xacro.process(xacro_file, mappings=mappings)
with open(output, 'w') as file_out:
file_out.write(doc)
# URDF spawner
args = ('-gazebo_namespace /gazebo '
'-x %s -y %s -z %s -R %s -P %s -Y %s -entity %s -file %s' %
(x, y, z, roll, pitch, yaw, namespace, output)).split()
# Urdf spawner. NB: node namespace does not prefix the topic,
# as using a leading /
urdf_spawner = Node(
node_name='urdf_spawner',
package='gazebo_ros',
node_executable='spawn_entity.py',
output='screen',
parameters=[{
'use_sim_time': res
}],
# To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}]
arguments=args)
# A joint state publisher plugin already is started with the model, no need to use the default joint state publisher
args = (output).split()
robot_state_publisher = Node(
node_name='robot_state_publisher',
package='robot_state_publisher',
node_executable='robot_state_publisher',
# To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}]
arguments=args,
output='screen',
parameters=[{
'use_sim_time': res
}] # Use subst here
)
# Message to tf
message_to_tf_launch = os.path.join(
get_package_share_directory('uuv_assistants'), 'launch',
'message_to_tf.launch')
if not pathlib.Path(message_to_tf_launch).exists():
exc = 'Launch file ' + message_to_tf_launch + ' does not exist'
raise Exception(exc)
launch_args = [('namespace', namespace), ('world_frame', 'world'),
('child_frame_id', '/' + namespace + '/base_link')]
message_to_tf_launch = IncludeLaunchDescription(
AnyLaunchDescriptionSource(message_to_tf_launch),
launch_arguments=launch_args)
group = GroupAction([
PushRosNamespace(namespace),
urdf_spawner,
robot_state_publisher,
])
return [group, message_to_tf_launch]
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 main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_body_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())
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = float(node.get_parameter('starting_time').value)
#starting time_clock = node.get_clock().now() + rclpy.duration.Duration(nanoseconds = starting_time * 1e9)
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
#print(force)
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))
service_name = '/gazebo/apply_link_wrench'
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
) #, rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.STEADY_TIME))
thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
#rclpy.spin_once(node)
#Just a guard for really short timeouts
if 1.0 / FREQ < starting_time:
rate.sleep()
# time.sleep(starting_time)
# if starting_time >= 0:
# rate = node.create_rate(100)
# while node.get_clock().now() < starting_time_clock:
# rate.sleep()
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=duration).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().info('Could not apply link wrench %r' %
(e, ))
else:
node.get_logger().info('Link wrench perturbation applied!')
node.get_logger().info('\tFrame: ' + body_name)
node.get_logger().info('\tDuration [s]: ' + str(duration))
node.get_logger().info('\tForce [N]: ' + str(force))
node.get_logger().info('\tTorque [Nm]: ' + str(torque))
break
