Skip to content

Commit 325fc69

Browse files
Q3rksesQ3rkses
authored
Vortex utility nodes (#47)
* added vortex_utility_nodes package, that should hold nodes useful for debugging or for missions * added readme for the euler_odom_publisher_node * fleshed out the implementation with launch file, config file and using a suitable message for publishing * updated readme to so it has correct launch command * made the node more general purpose * Cleaned up leftover old node name after refactor
1 parent b991606 commit 325fc69

6 files changed

Lines changed: 251 additions & 0 deletions

File tree

‎vortex_utility_nodes/CMakeLists.txt‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(vortex_utility_nodes)
3+
set(CMAKE_CXX_STANDARD 20)
4+
add_compile_options(-Wall -Wextra -Wpedantic)
5+
6+
find_package(ament_cmake REQUIRED)
7+
find_package(rclcpp REQUIRED)
8+
find_package(nav_msgs REQUIRED)
9+
find_package(vortex_msgs REQUIRED)
10+
find_package(vortex_utils REQUIRED)
11+
find_package(Eigen3 REQUIRED)
12+
13+
add_executable(message_publisher_node
14+
src/message_publisher.cpp
15+
)
16+
17+
ament_target_dependencies(message_publisher_node PUBLIC
18+
rclcpp
19+
nav_msgs
20+
Eigen3
21+
vortex_utils
22+
vortex_msgs
23+
)
24+
25+
install(TARGETS message_publisher_node
26+
DESTINATION lib/${PROJECT_NAME}
27+
)
28+
29+
install(
30+
DIRECTORY config/
31+
DESTINATION share/${PROJECT_NAME}/config
32+
)
33+
34+
install(
35+
DIRECTORY launch/
36+
DESTINATION share/${PROJECT_NAME}/launch
37+
)
38+
39+
ament_export_include_directories(include)
40+
ament_export_libraries(${PROJECT_NAME})
41+
42+
ament_package()

‎vortex_utility_nodes/README.md‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
# Vortex Utility Nodes
2+
3+
A collection of lightweight ROS2 utility nodes for testing, debugging, and operating the Vortex AUV.
4+
These are standalone helper nodes not core pipeline components. Use them to inspect data, convert formats, or bridge gaps during development and field operations.
5+
6+
## Nodes
7+
8+
| Node | Description |
9+
|------|-------------|
10+
| `euler_odometry_publisher_node` | Subscribes to `odometry` (resolved from config) and publishes odometry pose in Euler angles (roll, pitch, yaw). |
11+
12+
## Build
13+
```bash
14+
colcon build --packages-select vortex_utility_nodes
15+
source install/setup.bash
16+
```
17+
18+
## Run
19+
```bash
20+
ros2 launch vortex_utility_nodes euler_odometry_publisher_node.launch.py
21+
```
22+
23+
## Adding New Nodes
24+
25+
1. Create your node source file in `src/`
26+
2. Add an `add_executable` entry in `CMakeLists.txt`
27+
3. Add the install target to `lib/${PROJECT_NAME}`
28+
4. Update this table
29+
30+
## Current Dependencies
31+
32+
- `rclcpp`
33+
- `nav_msgs`
34+
- `vortex_msgs`
35+
- `vortex_utils`
36+
- `Eigen3`

‎vortex_utility_nodes/config/message_publisher.yaml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
message_publisher_node:
2+
ros__parameters:
3+
input_type: "odometry"
4+
topics:
5+
odom: "nautilus/odom"
6+
waypoint: "fsm/waypoint"
7+
reference_filter: "guidance/reference_filter"
8+
output: "utils/message_publisher/"

‎vortex_utility_nodes/launch/message_publisher.launch.py‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import Node
3+
from ament_index_python.packages import get_package_share_directory
4+
import os
5+
6+
def generate_launch_description():
7+
config = os.path.join(
8+
get_package_share_directory('vortex_utility_nodes'),
9+
'config',
10+
'message_publisher.yaml'
11+
)
12+
13+
return LaunchDescription([
14+
Node(
15+
package='vortex_utility_nodes',
16+
executable='message_publisher_node',
17+
name='message_publisher_node',
18+
parameters=[config],
19+
output='screen',
20+
),
21+
])

‎vortex_utility_nodes/package.xml‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>vortex_utility_nodes</name>
5+
<version>0.0.0</version>
6+
<description>Useful nodes for debugging and testing</description>
7+
<maintainer email="cyprian.github@gmail.com"> Cyprian Osinski</maintainer>
8+
<license>MIT</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>eigen</depend>
13+
<depend>yaml-cpp</depend>
14+
<depend>rclcpp</depend>
15+
<depend>nav_msgs</depend>
16+
<depend>vortex_msgs</depend>
17+
<depend>vortex_utils</depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
</export>
22+
</package>
23+

‎vortex_utility_nodes/src/message_publisher.cpp‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
#include <vortex/utils/math.hpp>
2+
#include "nav_msgs/msg/odometry.hpp"
3+
#include "rclcpp/rclcpp.hpp"
4+
#include "vortex_msgs/msg/reference_filter_quat.hpp"
5+
#include "vortex_msgs/msg/rpy.hpp"
6+
#include "vortex_msgs/msg/waypoint.hpp"
7+
8+
#include <variant>
9+
10+
class MessagePublisherNode : public rclcpp::Node {
11+
public:
12+
MessagePublisherNode() : Node("message_publisher_node") {
13+
this->declare_parameter<std::string>("input_type", "odometry");
14+
this->declare_parameter<std::string>("topics.odom", "nautilus/odom");
15+
this->declare_parameter<std::string>("topics.waypoint",
16+
"nautilus/waypoint");
17+
this->declare_parameter<std::string>("topics.reference_filter",
18+
"nautilus/reference_filter");
19+
this->declare_parameter<std::string>("topics.output",
20+
"utils/odometry/euler");
21+
22+
auto input_type = this->get_parameter("input_type").as_string();
23+
auto output_topic = this->get_parameter("topics.output").as_string();
24+
25+
rpy_pub_ =
26+
this->create_publisher<vortex_msgs::msg::RPY>(output_topic, 2);
27+
28+
if (input_type == "odometry") {
29+
auto topic = this->get_parameter("topics.odom").as_string();
30+
sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
31+
topic, 2,
32+
std::bind(&MessagePublisherNode::odom_callback, this,
33+
std::placeholders::_1));
34+
RCLCPP_INFO(this->get_logger(),
35+
"Euler publisher [odometry]: %s -> %s", topic.c_str(),
36+
output_topic.c_str());
37+
38+
} else if (input_type == "waypoint") {
39+
auto topic = this->get_parameter("topics.waypoint").as_string();
40+
sub_ = this->create_subscription<vortex_msgs::msg::Waypoint>(
41+
topic, 2,
42+
std::bind(&MessagePublisherNode::waypoint_callback, this,
43+
std::placeholders::_1));
44+
RCLCPP_INFO(this->get_logger(),
45+
"Euler publisher [waypoint]: %s -> %s", topic.c_str(),
46+
output_topic.c_str());
47+
48+
} else if (input_type == "reference_filter") {
49+
auto topic =
50+
this->get_parameter("topics.reference_filter").as_string();
51+
sub_ = this->create_subscription<
52+
vortex_msgs::msg::ReferenceFilterQuat>(
53+
topic, 2,
54+
std::bind(&MessagePublisherNode::ref_filter_callback, this,
55+
std::placeholders::_1));
56+
RCLCPP_INFO(this->get_logger(),
57+
"Euler publisher [reference_filter]: %s -> %s",
58+
topic.c_str(), output_topic.c_str());
59+
60+
} else {
61+
RCLCPP_FATAL(this->get_logger(),
62+
"Unknown input_type: '%s'. Must be 'odometry', "
63+
"'waypoint', or 'reference_filter'.",
64+
input_type.c_str());
65+
rclcpp::shutdown();
66+
return;
67+
}
68+
}
69+
70+
private:
71+
Eigen::Vector3d convert_quat(double w, double x, double y, double z) {
72+
Eigen::Quaterniond eigen_q(w, x, y, z);
73+
return vortex::utils::math::quat_to_euler(eigen_q);
74+
}
75+
76+
void publish_rpy(const std_msgs::msg::Header& header,
77+
const Eigen::Vector3d& euler) {
78+
vortex_msgs::msg::RPY msg;
79+
msg.header = header;
80+
msg.roll = euler.x();
81+
msg.pitch = euler.y();
82+
msg.yaw = euler.z();
83+
rpy_pub_->publish(msg);
84+
}
85+
86+
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
87+
const auto& q = msg->pose.pose.orientation;
88+
auto euler = convert_quat(q.w, q.x, q.y, q.z);
89+
publish_rpy(msg->header, euler);
90+
}
91+
92+
void waypoint_callback(const vortex_msgs::msg::Waypoint::SharedPtr msg) {
93+
const auto& q = msg->pose.orientation;
94+
auto euler = convert_quat(q.w, q.x, q.y, q.z);
95+
std_msgs::msg::Header header;
96+
header.stamp = this->now();
97+
publish_rpy(header, euler);
98+
}
99+
100+
void ref_filter_callback(
101+
const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) {
102+
auto euler = convert_quat(msg->qw, msg->qx, msg->qy, msg->qz);
103+
publish_rpy(msg->header, euler);
104+
}
105+
106+
using SubVariant = std::variant<
107+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr,
108+
rclcpp::Subscription<vortex_msgs::msg::Waypoint>::SharedPtr,
109+
rclcpp::Subscription<vortex_msgs::msg::ReferenceFilterQuat>::SharedPtr>;
110+
111+
SubVariant sub_;
112+
rclcpp::Publisher<vortex_msgs::msg::RPY>::SharedPtr rpy_pub_;
113+
};
114+
115+
int main(int argc, char** argv) {
116+
rclcpp::init(argc, argv);
117+
auto node = std::make_shared<MessagePublisherNode>();
118+
rclcpp::spin(node);
119+
rclcpp::shutdown();
120+
return 0;
121+
}

0 commit comments

Comments
 (0)