def launch(shutdown_pipe, package_name, launch_file_name, launch_arguments,
debug):
"""Launch a ROS system."""
event_loop = asyncio.new_event_loop()
asyncio.set_event_loop(event_loop)
# based on ros2launch/command/launch.py:main
if package_name is None:
single_file = True
else:
single_file = False
get_package_prefix(package_name)
path = None
launch_arguments = []
if single_file:
if os.path.exists(package_name):
path = package_name
else:
raise ValueError(package_name)
if launch_file_name is not None:
launch_arguments.append(launch_file_name)
else:
try:
path = get_share_file_path_from_package(package_name=package_name,
file_name=launch_file_name)
except PackageNotFoundError as exc:
raise RuntimeError("Package '{}' not found: {}".format(
package_name, exc))
except (FileNotFoundError, MultipleLaunchFilesError) as exc:
raise RuntimeError(str(exc))
launch_arguments.extend(launch_arguments)
launch_service = LaunchService(argv=launch_arguments, debug=debug)
launch_description = LaunchDescription([
IncludeLaunchDescription(
AnyLaunchDescriptionSource(path),
launch_arguments={},
),
])
launch_service.include_launch_description(launch_description)
finished = False
def shutdown():
while not finished and not shutdown_pipe.poll(1):
pass
launch_service.shutdown()
t = threading.Thread(target=shutdown)
t.start()
launch_service.run(shutdown_when_idle=True)
finished = True
t.join()
event_loop.close()
def __init__(self):
super().__init__('bike_alarm')
self.publisher_ = self.create_publisher(Marker, TOPIC_NAME_MARKERS, 10)
self.subscriber_imu = self.create_subscription(Imu,
TOPIC_NAME_LUOS_IMU,
self._cb_imu_received,
10)
self.subscriber_button = self.create_subscription(
BoolChange, TOPIC_NAME_LUOS_STATE, self._cb_state_received, 10)
self.timer_bike = self.create_timer(0.1, self._cb_timer_bike)
self.bike_color = BikeColor()
self.state = "idle" # The bike current state : ["idle", "ride", "alarm"]
# Here is the bike to be published in RViz
self.bike = Marker()
self.bike.type = Marker.MESH_RESOURCE
self.bike.header.frame_id = "/world"
self.bike.mesh_resource = "file://" + join(
get_package_prefix('luos_bike_alarm_example'),
RESOURCE_INDEX_SUBFOLDER, "packages", 'bike.stl')
self.bike.scale.x = 1e-3
self.bike.scale.y = 1e-3
self.bike.scale.z = 1e-3
self.bike.color.a = 1.
self.bike.pose.position.z = 3.
self.bike.pose.orientation.w = 1.
self.publisher_color = self.create_publisher(ColorRGBA,
TOPIC_NAME_LUOS_COLOR, 10)
self.get_logger().info("This is the Luos bike sharing example. \
Shake the IMU {} or press the State button {}".format(TOPIC_NAME_LUOS_IMU,
TOPIC_NAME_LUOS_STATE))
def generate_launch_description():
urdf = os.path.join(get_package_share_directory('mara_description'),
'urdf', 'mara_robot_camera_top.urdf')
mara = get_package_share_directory('mara_gazebo_plugins')
install_dir = get_package_prefix('mara_gazebo_plugins')
print("plugins", install_dir)
print("mara", mara)
if 'GAZEBO_MODEL_PATH' in os.environ:
os.environ['GAZEBO_MODEL_PATH'] = os.environ[
'GAZEBO_MODEL_PATH'] + ':' + install_dir + 'share'
else:
os.environ['GAZEBO_MODEL_PATH'] = install_dir + "/share"
if 'GAZEBO_PLUGIN_PATH' in os.environ:
os.environ['GAZEBO_PLUGIN_PATH'] = os.environ[
'GAZEBO_PLUGIN_PATH'] + ':' + install_dir + '/lib'
else:
os.environ['GAZEBO_PLUGIN_PATH'] = install_dir + '/lib'
try:
envs = {}
for key in os.environ.__dict__["_data"]:
key = key.decode("utf-8")
if (key.isupper()):
envs[key] = os.environ[key]
except Exception as e:
print("Error with Envs: " + str(e))
return None
ld = LaunchDescription([
ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
output='screen',
env=envs),
Node(package='robot_state_publisher',
node_executable='robot_state_publisher',
output='screen',
arguments=[urdf]),
Node(package='mara_utils_scripts',
node_executable='spawn_entity.py',
output='screen'),
Node(package='hros_cognition_mara_components',
node_executable='hros_cognition_mara_components',
output='screen',
arguments=[
"-motors", install_dir +
"/share/hros_cognition_mara_components/link_order.yaml"
]),
Node(package='individual_trajectories_bridge',
node_executable='individual_trajectories_bridge',
output='screen',
arguments=[
"-motors", install_dir +
"/share/individual_trajectories_bridge/motors.yaml"
])
])
return ld
def launchReal():
os.environ["ROS_DOMAIN_ID"] = str(22)
os.environ["RMW_IMPLEMENTATION"] = "rmw_opensplice_cpp"
installDir = get_package_prefix('mara_gazebo_plugins')
launchDesc = LaunchDescription([
Node(package='hros_cognition_mara_components',
node_executable='hros_cognition_mara_components',
arguments=["-motors", installDir \
+ "/share/hros_cognition_mara_components/link_order_real.yaml"], output='screen')
])
return launchDesc
def generate_launch_description():
use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time',
default='false')
params_file = launch.substitutions.LaunchConfiguration(
'params',
default=[
launch.substitutions.ThisLaunchFileDir(), '/nav2_params.yaml'
])
bt_navigator_install_path = get_package_prefix('nav2_bt_navigator')
bt_navigator_xml = os.path.join(
bt_navigator_install_path, 'behavior_trees',
'navigate_w_recovery_retry.xml'
) # TODO(mkhansen): change to an input parameter
return LaunchDescription([
launch.actions.DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
launch_ros.actions.Node(package='nav2_world_model',
node_executable='world_model',
output='screen',
parameters=[params_file]),
launch_ros.actions.Node(package='dwb_controller',
node_executable='dwb_controller',
output='screen',
parameters=[params_file]),
launch_ros.actions.Node(package='nav2_navfn_planner',
node_executable='navfn_planner',
node_name='navfn_planner',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}]),
launch_ros.actions.Node(package='nav2_recoveries',
node_executable='recoveries_node',
node_name='recoveries',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}]),
launch_ros.actions.Node(package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'bt_xml_filename': bt_navigator_xml
}])
])
def main(argv=sys.argv[1:]):
ld = generate_launch_description()
test1_action = ExecuteProcess(
cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_filter_base_diagnostics_timestamps'],
output='screen',
)
lts = LaunchTestService()
lts.add_test_action(ld, test1_action)
ls = LaunchService(argv=argv)
ls.include_launch_description(ld)
return lts.run(ls)
def generate_launch_description():
if not "tesseract_collision" in os.environ["AMENT_PREFIX_PATH"]:
# workaround for pluginlib ClassLoader bug: manually add tesseract_collision to the AMENT_PREFIX_PATH env variable
head, tail = os.path.split(get_package_prefix('crs_support'))
path = os.path.join(head, 'tesseract_collision')
os.environ["AMENT_PREFIX_PATH"] += os.pathsep + path
urdf = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'crs.urdf')
srdf = os.path.join(get_package_share_directory('crs_support'), 'urdf', 'ur10e_robot.srdf')
gzworld = os.path.join(get_package_share_directory('crs_support'), 'worlds', 'crs.world')
try:
crs_models_dir = str(Path.home().joinpath('.gazebo', 'models', 'crs_support').resolve(strict=True))
except FileNotFoundError: #os.path.exists(crs_models_dir):
gazebo_path = str(Path.home().joinpath('.gazebo', 'models').resolve())
os.mkdir(gazebo_path + "/crs_support")
shutil.copytree(os.path.join(get_package_share_directory('crs_support'), 'meshes'), Path.home().joinpath(Path.home().joinpath('.gazebo','models','crs_support','meshes').resolve()))
tesseract_env = launch_ros.actions.Node(
node_name='env_node',
package='tesseract_monitoring',
node_executable='tesseract_monitoring_environment_node',
output='screen',
parameters=[{'use_sim_time': 'true',
'desc_param': 'robot_description',
'robot_description': urdf,
'robot_description_semantic': srdf}])
gzserver = launch.actions.ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', '--world', gzworld],
output='screen'
)
spawner1 = launch_ros.actions.Node(
node_name='spawn_node',
package='gazebo_ros',
node_executable='spawn_entity.py',
arguments=['-entity', 'robot', '-x', '0', '-y', '0', '-z', '0.05', '-file', urdf])
return launch.LaunchDescription([
# environment
tesseract_env,
# gazebo
gzserver,
spawner1
])
def get(self, *args, **kwargs) -> None:
if not args:
self.write("Error - no argument supplied")
argslist = args[0].split('/')
package, rest = argslist[0], '/'.join(argslist[1:])
file = os.path.join(get_package_prefix(package), rest)
try:
with open(file, 'rb') as f:
data = f.read()
self.write(data)
except FileNotFoundError:
self.write("Error opening file %s" % file)
self.finish()
def generate_launch_description():
urdf = os.path.join(get_package_share_directory('mara_description'),
'urdf', 'mara_robot_gripper_140.urdf')
install_dir = get_package_prefix('mara_description')
print("install_dir ", install_dir)
if 'GAZEBO_MODEL_PATH' in os.environ:
os.environ['GAZEBO_MODEL_PATH'] = os.environ[
'GAZEBO_MODEL_PATH'] + ':' + install_dir + '/share'
else:
os.environ['GAZEBO_MODEL_PATH'] = install_dir + "/share"
if 'GAZEBO_PLUGIN_PATH' in os.environ:
os.environ['GAZEBO_PLUGIN_PATH'] = os.environ[
'GAZEBO_PLUGIN_PATH'] + ':' + install_dir + '/lib'
else:
os.environ['GAZEBO_PLUGIN_PATH'] = install_dir + '/lib'
try:
envs = {}
for key in os.environ.__dict__["_data"]:
key = key.decode("utf-8")
if (key.isupper()):
envs[key] = os.environ[key]
except Exception as e:
print("Error with Envs: " + str(e))
return None
rviz_config_path = os.path.join(
get_package_share_directory('mara_description'), 'rviz',
'visualization.rviz')
ld = LaunchDescription([
ExecuteProcess(cmd=['rviz2', '-d', rviz_config_path],
output='screen',
env=envs),
Node(package='robot_state_publisher',
node_executable='robot_state_publisher',
output='screen',
arguments=[urdf]),
Node(package='mara_description',
node_executable='mara_state_publisher.py',
output='screen'),
])
return ld
