Skip to content

Commit 91ea035

Browse files
ethanholterethanholter
committed
misc fixes, additional demo launch files
1 parent 7ea6f49 commit 91ea035

File tree

7 files changed

+276
-294
lines changed

7 files changed

+276
-294
lines changed

‎src/uirover_bringup/launch/rover.launch.py‎

Lines changed: 132 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,19 @@
11
import os
2-
from platform import node
3-
from time import strftime
42

53
os.environ["RCUTILS_COLORIZED_OUTPUT"] = "1"
64

75
from launch import LaunchDescription
8-
from launch.actions import IncludeLaunchDescription, ExecuteProcess
9-
from launch.launch_description_sources import AnyLaunchDescriptionSource
10-
from launch.substitutions import PathJoinSubstitution, Command, FindExecutable
6+
from launch.actions import IncludeLaunchDescription
7+
from launch.launch_description_sources import PythonLaunchDescriptionSource
8+
from launch.substitutions import PathJoinSubstitution
9+
10+
from ament_index_python.packages import get_package_share_directory
1111

1212
from launch_ros.actions import Node
1313
from launch_ros.substitutions import FindPackageShare
1414

15-
def generate_launch_description():
16-
17-
controller_config = PathJoinSubstitution(
18-
[FindPackageShare("uirover_description"), "config", "diff_drive.yaml"]
19-
)
2015

16+
def generate_launch_description():
2117
ublox_config = PathJoinSubstitution(
2218
[FindPackageShare("uirover_gnss"), "config", "zed_f9p.yaml"]
2319
)
@@ -44,27 +40,29 @@ def generate_launch_description():
4440

4541
try:
4642
i = 0
47-
for camera in os.listdir('/dev/Arducam'):
48-
if not camera.startswith('CAM'):
43+
for camera in os.listdir("/dev/Arducam"):
44+
if not camera.startswith("CAM"):
4945
continue
5046
index = int(camera[3:])
5147
nodes.append(
5248
Node(
53-
package="uirover_video",
49+
package="uirover_perception",
5450
executable="stream",
5551
name=f"arducam_{i}",
5652
output="both",
57-
parameters=[{
58-
'port': 5000 + index,
59-
'device': f"/dev/Arducam/{camera}",
60-
'host': '192.168.55.100',
61-
# 'width': 1280,
62-
# 'height': 720,
63-
'framerate': 20.0
64-
}]
53+
parameters=[
54+
{
55+
"port": 5000 + index,
56+
"device": f"/dev/Arducam/{camera}",
57+
"host": "192.168.55.100",
58+
# 'width': 1280,
59+
# 'height': 720,
60+
"framerate": 20.0,
61+
}
62+
],
6563
)
6664
)
67-
i+= 1
65+
i += 1
6866
except FileNotFoundError:
6967
pass
7068

@@ -77,43 +75,131 @@ def generate_launch_description():
7775
)
7876
)
7977

78+
# ============================
79+
# Visual SLAM
80+
# ============================
81+
8082
nodes.append(
8183
IncludeLaunchDescription(
82-
AnyLaunchDescriptionSource(
83-
PathJoinSubstitution(
84-
[FindPackageShare("realsense2_camera"), "launch/rs_launch.py"]
85-
)
84+
PythonLaunchDescriptionSource(
85+
[
86+
os.path.join(
87+
get_package_share_directory("realsense2_camera"), "launch"
88+
),
89+
"/rs_launch.py",
90+
]
8691
),
8792
launch_arguments={
88-
"camera_namespace": "uirover",
89-
"camera_name": "D435i_realsense_camera",
90-
"depth_module.depth_profile": "640x480x30",
91-
"pointcloud.enable": "true",
92-
"unite_imu_method": "2",
93-
"tf_publish_rate": "5.0",
93+
"camera_name": "realsense",
94+
"camera_namespace": "rover",
9495
"enable_gyro": "true",
9596
"enable_accel": "true",
97+
"unite_imu_method": "2",
9698
"enable_infra1": "true",
99+
"enable_infra2": "true",
100+
"enable_sync": "true",
101+
"initial_reset": "true",
97102
}.items(),
98-
)
103+
),
99104
)
100105

101106
nodes.append(
102107
Node(
103-
package="ublox_gps",
104-
executable="ublox_gps_node",
105-
name="ublox_gps_node",
106-
parameters=[ublox_config]
107-
)
108+
package="imu_filter_madgwick",
109+
executable="imu_filter_madgwick_node",
110+
namespace="rover",
111+
output="screen",
112+
parameters=[{"use_mag": False, "world_frame": "enu", "publish_tf": False}],
113+
remappings=[
114+
("imu/data_raw", "realsense/imu"),
115+
("imu/data", "realsense/imu/data"),
116+
],
117+
),
108118
)
109119

110120
nodes.append(
111121
Node(
112-
package='uirover_simple_hardware',
113-
executable='hardware_node'
122+
package="rtabmap_odom",
123+
executable="rgbd_odometry",
124+
namespace="rover/slam",
125+
output="screen",
126+
parameters=[
127+
{
128+
"frame_id": "realsense_link",
129+
"subscribe_depth": True,
130+
"subscribe_odom_info": True,
131+
"approx_sync": False,
132+
"wait_imu_to_init": True,
133+
}
134+
],
135+
remappings=[
136+
("imu", "/rover/realsense/imu/data"),
137+
("rgb/image", "/rover/realsense/infra1/image_rect_raw"),
138+
("rgb/camera_info", "/rover/realsense/infra1/camera_info"),
139+
("depth/image", "/rover/realsense/depth/image_rect_raw"),
140+
],
141+
),
142+
)
143+
144+
nodes.append(
145+
Node(
146+
package="rtabmap_slam",
147+
executable="rtabmap",
148+
namespace="rover/slam",
149+
output="screen",
150+
parameters=[
151+
{
152+
"frame_id": "realsense_link",
153+
"subscribe_depth": True,
154+
"subscribe_odom_info": True,
155+
"approx_sync": False,
156+
"wait_imu_to_init": True,
157+
}
158+
],
159+
remappings=[
160+
("imu", "/rover/realsense/imu/data"),
161+
("rgb/image", "/rover/realsense/infra1/image_rect_raw"),
162+
("rgb/camera_info", "/rover/realsense/infra1/camera_info"),
163+
("depth/image", "/rover/realsense/depth/image_rect_raw"),
164+
],
165+
arguments=["-d"],
166+
),
167+
)
168+
169+
nodes.append(
170+
Node(
171+
package="rtabmap_viz",
172+
executable="rtabmap_viz",
173+
namespace="rover/slam",
174+
output="screen",
175+
parameters=[
176+
{
177+
"frame_id": "realsense_link",
178+
"subscribe_depth": True,
179+
"subscribe_odom_info": True,
180+
"approx_sync": False,
181+
"wait_imu_to_init": True,
182+
}
183+
],
184+
remappings=[
185+
("rgb/image", "/rover/realsense/infra1/image_rect_raw"),
186+
("rgb/camera_info", "/rover/realsense/infra1/camera_info"),
187+
("depth/image", "/rover/realsense/depth/image_rect_raw"),
188+
],
189+
),
190+
)
191+
192+
nodes.append(
193+
Node(
194+
package="ublox_gps",
195+
executable="ublox_gps_node",
196+
name="ublox_gps_node",
197+
parameters=[ublox_config],
114198
)
115199
)
116200

201+
nodes.append(Node(package="uirover_simple_hardware", executable="hardware_node"))
202+
117203
nodes.append(
118204
Node(
119205
package="rmw_zenoh_cpp",
@@ -122,11 +208,11 @@ def generate_launch_description():
122208
)
123209
)
124210

125-
nodes.append(
126-
ExecuteProcess(
127-
cmd=f"ros2 bag record -o bag/{strftime('%Y-%m-%d-%H-%M-%S')} -a -d 9000".split(" "),
128-
output="log",
129-
)
130-
)
211+
# nodes.append(
212+
# ExecuteProcess(
213+
# cmd=f"ros2 bag record -o bag/{strftime('%Y-%m-%d-%H-%M-%S')} -a -d 9000".split(" "),
214+
# output="log",
215+
# )
216+
# )
131217

132218
return LaunchDescription(nodes)

‎src/uirover_bringup/launch/simulator.launch.py‎

Lines changed: 12 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,17 @@
1-
import os
21
from time import strftime
32

4-
# Setting env variables before importing ROS2 packages just in case they are read during import
5-
os.environ["ROS_AUTOMATIC_DISCOVERY_RANGE"] = "LOCALHOST"
6-
os.environ["RCUTILS_COLORIZED_OUTPUT"] = "1"
7-
83
from launch import LaunchDescription
94
from launch.actions import (
105
DeclareLaunchArgument,
11-
ExecuteProcess,
126
IncludeLaunchDescription,
13-
RegisterEventHandler,
7+
ExecuteProcess,
148
)
159
from launch.substitutions import (
1610
PathJoinSubstitution,
1711
LaunchConfiguration,
1812
FindExecutable,
1913
Command
2014
)
21-
from launch.event_handlers import OnProcessExit
2215
from launch.launch_description_sources import PythonLaunchDescriptionSource
2316

2417
from launch_ros.actions import Node
@@ -76,7 +69,6 @@ def generate_launch_description():
7669
(
7770
"gz_args",
7871
[
79-
# " -r -v 1 'https://fuel.gazebosim.org/1.0/Penkatron/worlds/Rubicon World' --physics-engine gz-physics-bullet-featherstone-plugin"
8072
" -r -v 1 'https://fuel.gazebosim.org/1.0/Penkatron/worlds/Rubicon World'"
8173
],
8274
)
@@ -114,18 +106,20 @@ def generate_launch_description():
114106
arguments=[
115107
"joint_state_broadcaster",
116108
"--switch-timeout",
117-
"20.0",
109+
"30.0",
118110
"--service-call-timeout",
119-
"20.0",
111+
"30.0",
120112
],
121113
)
114+
115+
122116
node_diff_drive_controller_spawner = Node(
123117
package="controller_manager",
124118
executable="spawner",
125119
arguments=[
126120
"diff_drive_base_controller",
127121
"--switch-timeout",
128-
"20.0",
122+
"30.0",
129123
"-p",
130124
controller_config,
131125
],
@@ -141,13 +135,16 @@ def generate_launch_description():
141135
package="foxglove_bridge",
142136
executable="foxglove_bridge",
143137
name="foxglove_bridge",
138+
namespace="basestation",
144139
)
145140
# higher deadzone makes it easier to drive straight
146141
node_gamepad_publisher = Node(
147142
package="joy",
148143
executable="joy_node",
149144
name="joy_node",
145+
namespace="basestation/gamepad",
150146
parameters=[{"coalesce_interval": 0.5, "deadzone": 0.20}],
147+
remappings=[("/joy", "/basestation/gamepad/joy")],
151148
)
152149
node_twist_publisher = Node(
153150
package="teleop_twist_joy",
@@ -168,7 +165,7 @@ def generate_launch_description():
168165
# ======= Processes ======= #
169166

170167
cmd_ros_bag = ExecuteProcess(
171-
cmd=f"ros2 bag record -o bag/{strftime('%Y-%m-%d-%H-%M-%S')}_gazebo -a -d 9000".split(
168+
cmd=f"ros2 bag record -o bag/{strftime('%Y-%m-%d-%H-%M-%S')}_gazebo -a -d 9000 --node-name bag".split(
172169
" "
173170
),
174171
output="screen",
@@ -185,18 +182,8 @@ def generate_launch_description():
185182
launch_description = LaunchDescription(
186183
[
187184
# Event handlers used to start nodes in a predictable order
188-
RegisterEventHandler(
189-
event_handler=OnProcessExit(
190-
target_action=node_gazebo_spawn_urdf,
191-
on_exit=[node_joint_state_broadcaster_spawner],
192-
)
193-
),
194-
RegisterEventHandler(
195-
event_handler=OnProcessExit(
196-
target_action=node_joint_state_broadcaster_spawner,
197-
on_exit=[node_diff_drive_controller_spawner],
198-
)
199-
),
185+
node_joint_state_broadcaster_spawner,
186+
node_diff_drive_controller_spawner,
200187
node_gazebo_spawn_urdf,
201188
launch_gazebo,
202189
node_robot_state_publisher,

‎src/uirover_description/urdf/uirover_sim.urdf.xacro‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<xacro:include filename="$(find uirover_description)/xacro/gazebo.urdf.xacro"/>
44

55
<xacro:include filename="$(find uirover_description)/xacro/ros2_control.urdf.xacro"/>
6-
<xacro:uirover_ros2_control use_gazebo="false"/>
6+
<xacro:uirover_ros2_control use_gazebo="true"/>
77

88
<xacro:include filename="$(find uirover_description)/xacro/chassis.urdf.xacro"/>
99

0 commit comments

Comments
 (0)