Skip to content

Commit dc5f8f9

Browse files
shayaf84shayaf84
committed
Merge with nav_testing
2 parents 76d7dc9 + 0b0f0e4 commit dc5f8f9

File tree

19 files changed

+790
-13
lines changed

19 files changed

+790
-13
lines changed

‎urc_bringup/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED)
1111
find_package(rosbridge_server REQUIRED)
1212
find_package(tf2_msgs REQUIRED)
1313
find_package(sensor_msgs REQUIRED)
14+
find_package(tf2_ros REQUIRED)
1415
find_package(urc_msgs REQUIRED)
1516

1617
include_directories(
@@ -20,6 +21,7 @@ include_directories(
2021
# Library creation
2122
add_library(${PROJECT_NAME} SHARED
2223
src/heartbeat_publisher.cpp
24+
src/rover_pose_bridge.cpp
2325
)
2426

2527
set(dependencies
@@ -29,6 +31,7 @@ set(dependencies
2931
rosbridge_server
3032
sensor_msgs
3133
tf2_msgs
34+
tf2_ros
3235
urc_msgs
3336
)
3437

@@ -43,6 +46,11 @@ rclcpp_components_register_node(
4346
EXECUTABLE ${PROJECT_NAME}_HeartbeatPublisher
4447
)
4548

49+
rclcpp_components_register_node(
50+
${PROJECT_NAME}
51+
PLUGIN "rover_pose_bridge::RoverPoseBridge"
52+
EXECUTABLE ${PROJECT_NAME}_RoverPoseBridge
53+
)
4654

4755
# Install launch files.
4856
install(

‎urc_bringup/config/sim_config.yaml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,4 +38,8 @@
3838
gz_topic_name: "/scan/points"
3939
ros_type_name: "sensor_msgs/msg/PointCloud2"
4040
gz_type_name: "ignition.msgs.PointCloudPacked"
41+
- ros_topic_name: "/ground_truth_pose"
42+
gz_topic_name: "/world/leo_marsyard/dynamic_pose/info"
43+
ros_type_name: "tf2_msgs/msg/TFMessage"
44+
gz_type_name: "ignition.msgs.Pose_V"
4145
direction: GZ_TO_ROS

‎urc_bringup/include/rover_pose_bridge.hpp‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
#ifndef ROVER_POSE_BRIDGE_HPP_
2+
#define ROVER_POSE_BRIDGE_HPP_
3+
4+
#include <rclcpp/rclcpp.hpp>
5+
#include <tf2_msgs/msg/tf_message.hpp>
6+
#include <urc_msgs/msg/rover_poses.hpp>
7+
#include <tf2_ros/transform_broadcaster.h>
8+
9+
10+
#include <string>
11+
12+
namespace rover_pose_bridge
13+
{
14+
15+
class RoverPoseBridge : public rclcpp::Node
16+
{
17+
public:
18+
explicit RoverPoseBridge(const rclcpp::NodeOptions & options);
19+
20+
private:
21+
void handleTransforms(const tf2_msgs::msg::TFMessage::SharedPtr msg) const;
22+
23+
rclcpp::Publisher<urc_msgs::msg::RoverPoses>::SharedPtr publisher_;
24+
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_;
25+
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
26+
27+
std::string target_child_frame_;
28+
};
29+
30+
} // namespace rover_pose_bridge
31+
32+
#endif // ROVER_POSE_BRIDGE_HPP_

‎urc_bringup/launch/sim.launch.py‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@ def generate_launch_description():
1414
path_ros_gazebo_sim = get_package_share_directory("ros_gz_sim")
1515
path_urc_hw_description = get_package_share_directory("urc_hw_description")
1616
path_urc_bringup = get_package_share_directory("urc_bringup")
17+
path_urc_localization = get_package_share_directory("urc_localization")
1718

1819
controller_config_file_dir = os.path.join(
1920
path_urc_bringup, "config", "test_controllers.yaml"
@@ -75,6 +76,20 @@ def generate_launch_description():
7576
launch_arguments={"gz_args": world_path}.items(),
7677
)
7778

79+
rover_pose_bridge = Node(
80+
package="urc_bringup",
81+
executable="urc_bringup_RoverPoseBridge",
82+
name="rover_pose_bridge",
83+
parameters=[
84+
{
85+
"tf_topic": "/ground_truth_pose",
86+
"rover_pos_topic": "/rover_ground_truth",
87+
"use_sim_time": True,
88+
}
89+
],
90+
output="screen",
91+
)
92+
7893
bridge = Node(
7994
package="ros_gz_bridge",
8095
executable="parameter_bridge",
@@ -138,9 +153,9 @@ def generate_launch_description():
138153
"-name",
139154
"walli",
140155
"-x",
141-
"0",
156+
"-20",
142157
"-y",
143-
"0",
158+
"-15",
144159
"-z",
145160
"1.5",
146161
"-R",
@@ -168,5 +183,6 @@ def generate_launch_description():
168183
load_joint_state_broadcaster,
169184
load_position_controller,
170185
load_velocity_controller,
186+
rover_pose_bridge,
171187
]
172188
)

‎urc_bringup/launch/sim.py‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
from launch import LaunchDescription
44
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
55
from launch.launch_description_sources import PythonLaunchDescriptionSource
6-
from launch.substitutions import LaunchConfiguration, Command
6+
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
77
from launch_ros.descriptions import ParameterValue
88
from ament_index_python.packages import get_package_share_directory
99
from launch_ros.actions import Node
@@ -21,8 +21,13 @@ def generate_launch_description():
2121

2222
sim_world_arg = DeclareLaunchArgument(
2323
"world",
24+
<<<<<<< HEAD
2425
default_value=os.path.join(path_urc_hw_description, "world", "marsyard2020.sdf"),
2526
description="Path to gz world file",
27+
=======
28+
default_value="marsyard2020.sdf",
29+
description="Name of the world file (not full path)",
30+
>>>>>>> origin/feat/nav_testing
2631
)
2732

2833
walli_xacro = DeclareLaunchArgument(
@@ -41,9 +46,14 @@ def generate_launch_description():
4146
description="bridge YAML config",
4247
)
4348

44-
world = LaunchConfiguration("world")
49+
world_filename = LaunchConfiguration("world")
4550
walli_xacro_config = LaunchConfiguration("walli_xacro")
4651

52+
# Construct the full world path from the filename
53+
world_path = PathJoinSubstitution(
54+
[path_urc_hw_description, "world", world_filename]
55+
)
56+
4757
"""
4858
robot_urdf_file = process_file(
4959
ParameterValue(walli_xacro_config, value_type = str),
@@ -67,7 +77,7 @@ def generate_launch_description():
6777
PythonLaunchDescriptionSource(
6878
os.path.join(path_ros_gazebo_sim, "launch", "gz_sim.launch.py")
6979
),
70-
launch_arguments={"gz_args": world}.items(),
80+
launch_arguments={"gz_args": world_path}.items(),
7181
)
7282

7383
bridge = Node(
@@ -137,7 +147,7 @@ def generate_launch_description():
137147
"-y",
138148
"0",
139149
"-z",
140-
"0.5",
150+
"1.5",
141151
"-R",
142152
"0",
143153
"-P",

‎urc_bringup/launch/sim_leo.py‎

Lines changed: 201 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,201 @@
1+
import os
2+
from tempfile import NamedTemporaryFile
3+
from launch import LaunchDescription
4+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
5+
from launch.launch_description_sources import PythonLaunchDescriptionSource
6+
from launch.substitutions import LaunchConfiguration, Command
7+
from launch_ros.descriptions import ParameterValue
8+
from ament_index_python.packages import get_package_share_directory
9+
from launch_ros.actions import Node
10+
from xacro import process_file
11+
12+
13+
def generate_launch_description():
14+
path_ros_gazebo_sim = get_package_share_directory("ros_gz_sim")
15+
path_urc_hw_description = get_package_share_directory("urc_hw_description")
16+
path_urc_bringup = get_package_share_directory("urc_bringup")
17+
18+
# <-- ADDED paths for the Leo Rover packages
19+
path_leo_description = get_package_share_directory("leo_description")
20+
path_leo_gz_bringup = get_package_share_directory("leo_gz_bringup")
21+
22+
# <-- CHANGED to use the Leo Rover's controller config
23+
controller_config_file_dir = os.path.join(
24+
path_leo_gz_bringup, "config", "controllers.yaml"
25+
)
26+
27+
# controller_config_file_dir = os.path.join(
28+
# path_urc_bringup, "config", "test_controllers.yaml"
29+
# )
30+
31+
sim_world_arg = DeclareLaunchArgument(
32+
"world",
33+
# default_value=os.path.join(path_urc_hw_description, "world", "leo_world.sdf"),
34+
default_value=os.path.join(path_urc_hw_description, "world", "marsyard2020.sdf"),
35+
description="Path to gz world file",
36+
)
37+
38+
# <-- CHANGED default_value to point to the Leo Rover's xacro file
39+
# (I kept your variable name 'walli_xacro' for minimal changes,
40+
# but you could rename it to 'robot_xacro')
41+
walli_xacro = DeclareLaunchArgument(
42+
"walli_xacro",
43+
default_value=os.path.join(
44+
path_leo_description,
45+
"urdf",
46+
"leo.urdf.xacro",
47+
),
48+
description="Path to xacro file",
49+
)
50+
# walli_xacro = DeclareLaunchArgument(
51+
# "walli_xacro",
52+
# default_value=os.path.join(
53+
# path_urc_hw_description,
54+
# "urdf/simplified_swerve",
55+
# "simplified_swerve.urdf.xacro",
56+
# ),
57+
# description="Path to xacro file",
58+
# )
59+
60+
bridge_yaml = DeclareLaunchArgument(
61+
"bridge_yaml",
62+
default_value=os.path.join(path_urc_bringup, "config", "sim_config.yaml"),
63+
description="bridge YAML config",
64+
)
65+
66+
world = LaunchConfiguration("world")
67+
walli_xacro_config = LaunchConfiguration("walli_xacro")
68+
69+
"""
70+
robot_urdf_file = process_file(
71+
ParameterValue(walli_xacro_config, value_type = str),
72+
mappings = {"use_sim": "true"}
73+
).toxml()
74+
"""
75+
76+
robot_urdf_file = ParameterValue(
77+
Command(
78+
[
79+
"xacro ",
80+
walli_xacro_config,
81+
" use_sim:=",
82+
"true",
83+
]
84+
),
85+
value_type=str,
86+
)
87+
88+
gz_sim = IncludeLaunchDescription(
89+
PythonLaunchDescriptionSource(
90+
os.path.join(path_ros_gazebo_sim, "launch", "gz_sim.launch.py")
91+
),
92+
launch_arguments={"gz_args": world}.items(),
93+
)
94+
95+
bridge = Node(
96+
package="ros_gz_bridge",
97+
executable="parameter_bridge",
98+
name="ros_gz_bridge",
99+
output="screen",
100+
parameters=[{"config_file": LaunchConfiguration("bridge_yaml")}],
101+
)
102+
103+
robot_state_publisher_node = Node(
104+
package="robot_state_publisher",
105+
executable="robot_state_publisher",
106+
name="robot_state_publisher",
107+
parameters=[{"robot_description": robot_urdf_file}],
108+
output="screen",
109+
)
110+
111+
control_node = Node(
112+
package="controller_manager",
113+
executable="ros2_control_node",
114+
parameters=[controller_config_file_dir],
115+
output="screen",
116+
remappings=[("/controller_manager/robot_description", "/robot_description")],
117+
)
118+
119+
# <-- ADDED spawner for the Leo Rover's differential drive controller
120+
load_diff_drive_controller = Node(
121+
package="controller_manager",
122+
executable="spawner",
123+
arguments=["-p", controller_config_file_dir, "diff_drive_controller"],
124+
)
125+
load_joint_state_broadcaster = Node(
126+
package="controller_manager",
127+
executable="spawner",
128+
arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"],
129+
)
130+
131+
# load_position_controller = Node(
132+
# package="controller_manager",
133+
# executable="spawner",
134+
# arguments=["-p", controller_config_file_dir, "position_controller"],
135+
# )
136+
137+
# load_velocity_controller = Node(
138+
# package="controller_manager",
139+
# executable="spawner",
140+
# arguments=["-p", controller_config_file_dir, "velocity_controller"],
141+
# )
142+
143+
spawn = Node(
144+
package="ros_gz_sim",
145+
executable="create",
146+
output="screen",
147+
arguments=[
148+
"-name",
149+
"leo", # "walli",
150+
"-x",
151+
"0",
152+
"-y",
153+
"0",
154+
"-z",
155+
"10.5",
156+
"-R",
157+
"0",
158+
"-P",
159+
"0",
160+
"-Y",
161+
"0",
162+
"-topic",
163+
"robot_description",
164+
],
165+
)
166+
167+
return LaunchDescription(
168+
[
169+
sim_world_arg,
170+
walli_xacro,
171+
gz_sim,
172+
spawn,
173+
bridge_yaml,
174+
bridge,
175+
control_node,
176+
robot_state_publisher_node,
177+
load_joint_state_broadcaster,
178+
load_diff_drive_controller, # <-- ADDED
179+
# <-- REMOVED old controllers
180+
]
181+
)
182+
183+
# return LaunchDescription(
184+
# [
185+
# sim_world_arg,
186+
# walli_xacro,
187+
# gz_sim,
188+
# spawn,
189+
# bridge_yaml,
190+
# bridge,
191+
# control_node,
192+
# robot_state_publisher_node,
193+
# load_joint_state_broadcaster,
194+
# load_position_controller,
195+
# load_velocity_controller,
196+
# ]
197+
# )
198+
199+
200+
201+

0 commit comments

Comments
 (0)