def generate_test_description():
ld = LaunchDescription()
pkg_name = 'triton_example'
components = ['triton_example::ComponentOne', 'triton_example::ComponentTwo']
example_container = ComposableNodeContainer(
name='example_container',
namespace='/',
package='rclcpp_components',
executable='component_container',
output='screen'
)
# There is a bug when using launch_test with ComposableNode, need to use cli
load_processes = {}
load_actions = []
for component in components:
load_component = ExecuteProcess(
cmd=['ros2', 'component', 'load' ,'/example_container', pkg_name, component]
)
name = component[component.index("::")+2:].lower()
load_processes[name] = load_component
load_actions.append(load_component)
# Delay so container can start up before loading
delayed_load = TimerAction(
period=3.0,
actions=load_actions
)
ld.add_action(example_container)
ld.add_action(delayed_load)
ld.add_action(launch_testing.actions.ReadyToTest())
return ld, load_processes
def generate_launch_description():
ld = LaunchDescription()
pkg_share = get_package_share_directory('triton_gazebo')
urdf_file = os.path.join(pkg_share, 'gazebo', 'models', 'cube_auv',
'model.urdf')
with open(urdf_file, 'r') as infp:
robot_desc = infp.read()
rsp_params = {'robot_description': robot_desc}
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('triton_gazebo'),
'launch', 'gazebo_launch.py')),
launch_arguments={'world': 'competition.world'}.items())
rviz = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('triton_gazebo'),
'launch', 'rviz_launch.py')))
rviz_timer = TimerAction(period=5., actions=[rviz])
state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[rsp_params])
transform_publisher = Node(package='triton_controls',
executable='auv_transform_publisher.py',
name='auv_transform_publisher',
output='screen')
thrust_allocator = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('triton_controls'),
'launch', 'thrust_allocator_launch.py')))
keyboard_teleop = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('triton_teleop'),
'launch', 'keyboard_teleop_launch.py')))
ld.add_action(gazebo)
ld.add_action(rviz_timer)
ld.add_action(thrust_allocator)
ld.add_action(keyboard_teleop)
ld.add_action(state_publisher)
ld.add_action(transform_publisher)
return ld
def generate_launch_description():
return LaunchDescription([
ExecuteLocal(
process_description=Executable(cmd=[sys.executable, '-c', "print('action')"]),
respawn=True, respawn_delay=respawn_delay, on_exit=on_exit_callback
),
TimerAction(
period=shutdown_time,
actions=[
Shutdown(reason='Timer expired')
]
)
])
def generate_launch_description():
# launch gazebo
list_description = [
ExecuteProcess(cmd=['gazebo', '-s', 'libgazebo_ros_factory.so'],
output='screen')
]
# set use_sim_time = true after 5 seconds
action = ExecuteProcess(
cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', 'true'],
output='screen')
list_description.append(TimerAction(period=5.0, actions=[action]))
return LaunchDescription(list_description)
def generate_launch_description():
# Path
gazebo_launch_dir = os.path.join(
get_package_share_directory('neuronbot2_gazebo'), 'launch')
nb2nav_launch_dir = os.path.join(
get_package_share_directory('neuronbot2_nav'), 'launch')
nb2nav_map_dir = os.path.join(
get_package_share_directory('neuronbot2_nav'), 'map')
# Parameters
use_sim_time = LaunchConfiguration('use_sim_time', default='True')
open_rviz = LaunchConfiguration('open_rviz', default='True')
## mememan_world.model / phenix_world.model
world_model = LaunchConfiguration('world_model',
default='mememan_world.model')
## mememan.yaml / phenix.yaml
map_path = LaunchConfiguration('map',
default=nb2nav_map_dir + '/mememan.yaml')
gazebo_world_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(gazebo_launch_dir, 'neuronbot2_world.launch.py')),
launch_arguments={
'use_sim_time': use_sim_time,
'world_model': world_model
}.items())
navigation_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(nb2nav_launch_dir, 'bringup_launch.py')),
launch_arguments={
'use_sim_time':
use_sim_time,
'open_rviz': open_rviz,
'map': map_path
}.items())
delayed_launch = TimerAction(
actions=[navigation_launch],
period=8.0 # delay 8 sec to make sure gazebo is ready
)
ld = LaunchDescription()
ld.add_action(gazebo_world_launch)
ld.add_action(delayed_launch)
return ld
def generate_test_description():
share_dir = FindPackageShare('xacro_live')
spawn_launch_path = PathJoinSubstitution(
[share_dir, 'launch/xacro_live_view.launch.py'])
xacro_file_path = str(Path(__file__).parent / 'urdf/robot.xacro')
spawn_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(spawn_launch_path),
launch_arguments={'xacro_file': xacro_file_path}.items(),
)
return (launch.LaunchDescription([
spawn_launch,
TimerAction(period=5., actions=[ReadyToTest()]),
]), {
'spawn_launch': spawn_launch
})
def generate_launch_description():
yaml_file_name = 'teams_params.yaml'
yaml_file = os.path.join(
get_package_share_directory('team_ijnek_launch'),
'config',
yaml_file_name)
with open(yaml_file, 'r') as infp:
teams_params = yaml.load(infp.read(), Loader=yaml.FullLoader)
ld = LaunchDescription()
# We must launch the agents one after another, or the server crashes
# Try and have a one second delay between the agent launches
delay_seconds = 0
for team, team_params in teams_params.items():
print('Found team: ' + str(team))
print(team_params)
for player, player_params in team_params.items():
print('Found player: ' + str(player))
print(player_params)
ld.add_action(
TimerAction(period=str(delay_seconds), actions=[
IncludeLaunchDescription(
PythonLaunchDescriptionSource([
get_package_share_directory('team_ijnek_launch'),
'/launch', '/simulated_player_launch.py']),
launch_arguments={
'namespace': str(team) + '/player' + str(player),
'team': str(team),
'unum': str(player),
'x': str(player_params['x']),
'y': str(player_params['y']),
'theta': str(player_params['theta']),
}.items(),
),
]))
delay_seconds = delay_seconds + 1
return ld
def generate_test_description():
ld = LaunchDescription()
pkg_name = 'triton_object_recognition'
component = 'triton_object_recognition::ObjectRecognizer'
config = os.path.join(
get_package_share_directory('triton_object_recognition'),
'config',
'tiny_yolov3.yaml'
)
object_recognizer_container = ComposableNodeContainer(
name='object_recognizer_container',
namespace='/',
package='rclcpp_components',
executable='component_container',
output='screen'
)
# There is a bug when using launch_test with ComposableNode, need to use cli
load_processes = {}
load_actions = []
load_component = ExecuteProcess(
cmd=['ros2', 'component', 'load' ,'/object_recognizer_container', pkg_name, component]
)
load_processes["objectrecognizer"] = load_component
load_actions.append(load_component)
# Delay so container can start up before loading
delayed_load = TimerAction(
period=3.0,
actions=load_actions
)
ld.add_action(object_recognizer_container)
ld.add_action(delayed_load)
ld.add_action(launch_testing.actions.ReadyToTest())
return ld, load_processes
def execute(self, context):
"""
Execute the action.
Delegated to :meth:`launch.actions.ExecuteProcess.execute`.
"""
actions = super().execute(context)
if not self.__timeout:
return actions
self.__timer = TimerAction(period=self.__timeout,
actions=[
OpaqueFunction(
function=self._shutdown_process,
kwargs={'send_sigint': True})
])
on_process_exit_event = OnProcessExit(on_exit=self.__on_process_exit,
target_action=self)
context.register_event_handler(on_process_exit_event)
if not actions:
return [self.__timer]
return actions.append(self.__timer)
def generate_servo_test_description(
*args,
gtest_name: SomeSubstitutionsType,
start_position_path: SomeSubstitutionsType = ""):
# Get parameters using the demo config file
servo_yaml = load_yaml("moveit_servo",
"config/panda_simulated_config.yaml")
servo_params = {"moveit_servo": servo_yaml}
# Get URDF and SRDF
if start_position_path:
initial_positions_file = os.path.join(os.path.dirname(__file__),
start_position_path)
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
),
mappings={"initial_positions_file": initial_positions_file},
)
else:
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner.py {}".format(
controller)
],
shell=True,
output="screen",
)
]
# Component nodes for tf and Servo
test_container = ComposableNodeContainer(
name="test_servo_integration_container",
namespace="/",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="robot_state_publisher",
plugin="robot_state_publisher::RobotStatePublisher",
name="robot_state_publisher",
parameters=[robot_description],
),
ComposableNode(
package="tf2_ros",
plugin="tf2_ros::StaticTransformBroadcasterNode",
name="static_tf2_broadcaster",
parameters=[{
"/child_frame_id": "panda_link0",
"/frame_id": "world"
}],
),
ComposableNode(
package="moveit_servo",
plugin="moveit_servo::ServoServer",
name="servo_server",
parameters=[
servo_params,
robot_description,
robot_description_semantic,
],
extra_arguments=[{
"use_intra_process_comm": True
}],
),
],
output="screen",
)
# Unknown how to set timeout
# https://github.com/ros2/launch/issues/466
servo_gtest = launch_ros.actions.Node(
executable=PathJoinSubstitution(
[LaunchConfiguration("test_binary_dir"), gtest_name]),
parameters=[servo_params],
output="screen",
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
ros2_control_node,
test_container,
TimerAction(period=2.0, actions=[servo_gtest]),
launch_testing.actions.ReadyToTest(),
] + load_controllers), {
"test_container": test_container,
"servo_gtest": servo_gtest,
"ros2_control_node": ros2_control_node,
}
def generate_move_group_test_description(*args,
gtest_name: SomeSubstitutionsType):
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": kinematics_yaml
}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters":
"""default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_yaml["panda_arm"]["enforce_constrained_state_space"] = True
ompl_planning_yaml["panda_arm"][
"projection_evaluator"] = "joints(panda_joint1,panda_joint2)"
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config",
"config/panda_controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager":
moveit_simple_controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.5,
"trajectory_execution.allowed_goal_duration_margin": 1.0,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# Static TF
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=[
"0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"
],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner.py {}".format(
controller)
],
shell=True,
output="screen",
)
]
# test executable
ompl_constraint_test = launch_ros.actions.Node(
executable=PathJoinSubstitution(
[LaunchConfiguration("test_binary_dir"), gtest_name]),
parameters=[
robot_description, robot_description_semantic, kinematics_yaml
],
output="screen",
)
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
run_move_group_node,
static_tf,
robot_state_publisher,
ros2_control_node,
TimerAction(period=2.0, actions=[ompl_constraint_test]),
launch_testing.actions.ReadyToTest(),
] + load_controllers), {
"run_move_group_node": run_move_group_node,
"static_tf": static_tf,
"robot_state_publisher": robot_state_publisher,
"ros2_control_node": ros2_control_node,
"ompl_constraint_test": ompl_constraint_test,
}
def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType):
# Get URDF and SRDF
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
)
)
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
# Get parameters for the Pose Tracking node
pose_tracking_yaml = load_yaml("moveit_servo", "config/pose_tracking_settings.yaml")
pose_tracking_params = {"moveit_servo": pose_tracking_yaml}
# Get parameters for the Servo node
servo_yaml = load_yaml(
"moveit_servo", "config/panda_simulated_config_pose_tracking.yaml"
)
servo_params = {"moveit_servo": servo_yaml}
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"panda_ros_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
shell=True,
output="screen",
)
]
# Component nodes for tf and Servo
test_container = ComposableNodeContainer(
name="test_pose_tracking_container",
namespace="/",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[
ComposableNode(
package="robot_state_publisher",
plugin="robot_state_publisher::RobotStatePublisher",
name="robot_state_publisher",
parameters=[robot_description],
),
ComposableNode(
package="tf2_ros",
plugin="tf2_ros::StaticTransformBroadcasterNode",
name="static_tf2_broadcaster",
parameters=[{"/child_frame_id": "panda_link0", "/frame_id": "world"}],
),
],
output="screen",
)
pose_tracking_gtest = launch_ros.actions.Node(
executable=PathJoinSubstitution(
[LaunchConfiguration("test_binary_dir"), gtest_name]
),
parameters=[
robot_description,
robot_description_semantic,
pose_tracking_params,
servo_params,
kinematics_yaml,
],
output="screen",
)
return launch.LaunchDescription(
[
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
ros2_control_node,
test_container,
TimerAction(period=2.0, actions=[pose_tracking_gtest]),
launch_testing.actions.ReadyToTest(),
]
+ load_controllers
), {
"test_container": test_container,
"servo_gtest": pose_tracking_gtest,
"ros2_control_node": ros2_control_node,
}
def generate_launch_description():
ap = argparse.ArgumentParser(
prog='ros2 launch choirbot_examples formationcontrol.launch.py')
ap.add_argument("-l",
"--length",
help="length of hexagon sides",
default=3,
type=float)
ap.add_argument("-s",
"--seed",
help="seed for initial positions",
default=5,
type=float)
# parse arguments (exception thrown on error)
args, _ = ap.parse_known_args(sys.argv)
L = float(args.length)
# set rng seed
np.random.seed(args.seed)
# communication matrix
N = 6
Adj = np.array([ # alternated zeros and ones
[0, 1, 0, 1, 0, 1], [1, 0, 1, 0, 1, 0], [0, 1, 0, 1, 0, 1],
[1, 0, 1, 0, 1, 0], [0, 1, 0, 1, 0, 1], [1, 0, 1, 0, 1, 0]
])
# generate matrix of desired inter-robot distances
# adjacent robots have distance L
# opposite robots have distance 2L
W = np.array([[0, L, 0, 2 * L, 0, L], [L, 0, L, 0, 2 * L, 0],
[0, L, 0, L, 0, 2 * L], [2 * L, 0, L, 0, L, 0],
[0, 2 * L, 0, L, 0, L], [L, 0, 2 * L, 0, L, 0]])
# generate coordinates of hexagon with center in the origin
a = L / 2
b = np.sqrt(3) * a
P = np.array([[-b, a, 0], [0, 2.0 * a, 0], [b, a, 0], [b, -a, 0],
[0, -2.0 * a, 0], [-b, -a, 0]])
# initial positions have a perturbation of at most L/3
P += np.random.uniform(-L / 3, L / 3, (6, 3))
# initialize launch description
robot_launch = [] # launched after 10 sec (to let Gazebo open)
launch_description = [] # launched immediately (will contain robot_launch)
# add executables for each robot
for i in range(N):
in_neighbors = np.nonzero(Adj[:, i])[0].tolist()
out_neighbors = np.nonzero(Adj[i, :])[0].tolist()
weights = W[i, :].tolist()
position = P[i, :].tolist()
# guidance
robot_launch.append(
Node(package='choirbot_examples',
node_executable='choirbot_formationcontrol_guidance',
output='screen',
node_namespace='agent_{}'.format(i),
parameters=[{
'agent_id': i,
'N': N,
'in_neigh': in_neighbors,
'out_neigh': out_neighbors,
'weights': weights
}]))
# controller
robot_launch.append(
Node(package='choirbot_examples',
node_executable='choirbot_formationcontrol_controller',
output='screen',
node_namespace='agent_{}'.format(i),
parameters=[{
'agent_id': i
}]))
# turtlebot spawner
launch_description.append(
Node(package='choirbot_examples',
node_executable='choirbot_turtlebot_spawner',
output='screen',
parameters=[{
'namespace': 'agent_{}'.format(i),
'position': position
}]))
# include launcher for gazebo
gazebo_launcher = os.path.join(
get_package_share_directory('choirbot_examples'), 'gazebo.launch.py')
launch_description.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(gazebo_launcher)))
# include delayed robot executables
timer_action = TimerAction(period=10.0,
actions=[LaunchDescription(robot_launch)])
launch_description.append(timer_action)
return LaunchDescription(launch_description)
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
rviz_file = "skidbot.rviz"
robot_file = "skidbot.urdf"
package_name = "gcamp_gazebo"
world_file_name = "gcamp_world.world"
pkg_path = os.path.join(get_package_share_directory(package_name))
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
urdf_file = os.path.join(pkg_path, "urdf", robot_file)
rviz_config = os.path.join(pkg_path, "rviz", rviz_file)
world_path = os.path.join(pkg_path, "worlds", world_file_name)
# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
launch_arguments={'world': world_path}.items()
)
# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
)
# Robot State Publisher
doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]
)
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
output='screen',
arguments=['-topic', 'robot_description', '-entity', 'skidbot'],
)
rviz_start = ExecuteProcess(
cmd=["ros2", "run", "rviz2", "rviz2", "-d", rviz_config], output="screen"
)
# create and return launch description object
return LaunchDescription(
[
TimerAction(
period=3.0,
actions=[rviz_start]
),
# start gazebo, notice we are using libgazebo_ros_factory.so instead of libgazebo_ros_init.so
# That is because only libgazebo_ros_factory.so contains the service call to /spawn_entity
# ExecuteProcess(
# cmd=["gazebo", "--verbose", world_path, "-s", "libgazebo_ros_factory.so"],
# output="screen",
# ),
start_gazebo_server_cmd,
start_gazebo_client_cmd,
robot_state_publisher_node,
# tell gazebo to spwan your robot in the world by calling service
spawn_entity,
]
)
def generate_launch_description():
ap = argparse.ArgumentParser(
prog='ros2 launch choirbot_examples taskassignment.launch.py')
ap.add_argument("-n",
"--number",
help="number of robots",
default=4,
type=int)
ap.add_argument("-s",
"--seed",
help="seed for initial positions",
default=3,
type=int)
# parse arguments (exception thrown on error)
args, _ = ap.parse_known_args(sys.argv)
N = args.number
# generate communication graph (this function also sets the seed)
Adj = binomial_random_graph(N, 0.2, seed=args.seed)
# generate initial positions in [-3, 3] with z = 0
P = np.zeros((N, 3))
P[:, 0:2] = np.random.randint(-3, 3, (N, 2))
# initialize launch description
robot_launch = [] # launched after 10 sec (to let Gazebo open)
launch_description = [] # launched immediately (will contain robot_launch)
# add task table executable
robot_launch.append(
Node(package='choirbot_examples',
node_executable='choirbot_taskassignment_table',
output='screen',
prefix=['xterm -hold -e'],
parameters=[{
'N': N
}]))
# add executables for each robot
for i in range(N):
in_neighbors = np.nonzero(Adj[:, i])[0].tolist()
out_neighbors = np.nonzero(Adj[i, :])[0].tolist()
position = P[i, :].tolist()
# guidance
robot_launch.append(
Node(package='choirbot_examples',
node_executable='choirbot_taskassignment_guidance',
output='screen',
prefix=['xterm -hold -e'],
node_namespace='agent_{}'.format(i),
parameters=[{
'agent_id': i,
'N': N,
'in_neigh': in_neighbors,
'out_neigh': out_neighbors
}]))
# planner
robot_launch.append(
Node(package='choirbot_examples',
node_executable='choirbot_taskassignment_planner',
output='screen',
node_namespace='agent_{}'.format(i),
parameters=[{
'agent_id': i
}]))
# controller
robot_launch.append(
Node(package='choirbot_examples',
node_executable='choirbot_taskassignment_controller',
output='screen',
node_namespace='agent_{}'.format(i),
parameters=[{
'agent_id': i
}]))
# turtlebot spawner
launch_description.append(
Node(package='choirbot_examples',
node_executable='choirbot_turtlebot_spawner',
output='screen',
parameters=[{
'namespace': 'agent_{}'.format(i),
'position': position
}]))
# include launcher for gazebo
gazebo_launcher = os.path.join(
get_package_share_directory('choirbot_examples'), 'gazebo.launch.py')
launch_description.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(gazebo_launcher)))
# include delayed robot executables
timer_action = TimerAction(period=10.0,
actions=[LaunchDescription(robot_launch)])
launch_description.append(timer_action)
return LaunchDescription(launch_description)
def generate_launch_description():
# Path
gazebo_launch_dir = os.path.join(
get_package_share_directory('neuronbot2_gazebo'), 'launch')
nb2nav_launch_dir = os.path.join(
get_package_share_directory('neuronbot2_nav'), 'launch')
nb2nav_map_dir = os.path.join(
get_package_share_directory('neuronbot2_nav'), 'map')
# Parameters
use_sim_time = LaunchConfiguration('use_sim_time', default='True')
open_rviz = LaunchConfiguration('open_rviz', default='True')
## mememan_world.model / phenix_world.model
world_model = LaunchConfiguration('world_model',
default='mememan_world.model')
## mememan.yaml / phenix.yaml
map_path = LaunchConfiguration('map',
default=nb2nav_map_dir + '/mememan.yaml')
use_camera = LaunchConfiguration('use_camera', default='top')
gazebo_world_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(gazebo_launch_dir, 'neuronbot2_world.launch.py')),
launch_arguments={
'use_sim_time': use_sim_time,
'world_model': world_model,
'use_camera': use_camera
}.items())
navigation_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(nb2nav_launch_dir, 'bringup_launch.py')),
launch_arguments={
'use_sim_time':
use_sim_time,
'open_rviz': open_rviz,
'map': map_path
}.items())
delayed_launch = TimerAction(
actions=[navigation_launch],
period=8.0 # delay 8 sec to make sure gazebo is ready
)
image_saver = Node(package='image_view',
executable='image_saver',
remappings=[('image', 'camera_color_frame/image_raw')],
parameters=[{
'use_sim_time':
True,
'save_all_image':
False,
'filename_format':
str(Path.home()) +
"/neuron_app_inspection/your_photo%04d.%s"
}],
output='screen')
ld = LaunchDescription()
ld.add_action(gazebo_world_launch)
ld.add_action(delayed_launch)
ld.add_action(image_saver)
return ld