diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index 1202295..58b5fad 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -43,9 +43,9 @@ class FixpositionDriverNode : public FixpositionDriver { void RegisterObservers(); - void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg); + void WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg); - void RtcmCallback(const fixposition_driver_ros1::RTCMConstPtr& msg); + void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg); private: /** diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp index 8559665..03c5e3d 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp @@ -29,7 +29,9 @@ #include #include #include -#include + +// Include RTCM +#include // Include extras #include diff --git a/fixposition_driver_ros1/launch/serial.yaml b/fixposition_driver_ros1/launch/serial.yaml index dba8719..62f9ff0 100644 --- a/fixposition_driver_ros1/launch/serial.yaml +++ b/fixposition_driver_ros1/launch/serial.yaml @@ -14,4 +14,4 @@ fp_output: customer_input: speed_topic: "/fixposition/speed" - rtcm_topic: "/fixposition/rtcm" + rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index ae7aaa7..1f7745f 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -14,4 +14,4 @@ fp_output: customer_input: speed_topic: "/fixposition/speed" - rtcm_topic: "/fixposition/rtcm" + rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros1/package.xml b/fixposition_driver_ros1/package.xml index 48d3539..7956e19 100644 --- a/fixposition_driver_ros1/package.xml +++ b/fixposition_driver_ros1/package.xml @@ -18,6 +18,7 @@ message_generation message_runtime fixposition_driver_lib + rtcm_msgs tf2_ros tf2_geometry_msgs diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 3692ddf..a88ef7f 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -66,11 +66,11 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para { ws_sub_ = nh_.subscribe(params_.customer_input.speed_topic, 10, - &FixpositionDriverNode::WsCallback, this, + &FixpositionDriverNode::WsCallbackRos, this, ros::TransportHints().tcpNoDelay()); - rtcm_sub_ = nh_.subscribe(params_.customer_input.rtcm_topic, 10, - &FixpositionDriverNode::RtcmCallback, this, - ros::TransportHints().tcpNoDelay()); + rtcm_sub_ = nh_.subscribe(params_.customer_input.rtcm_topic, 10, + &FixpositionDriverNode::RtcmCallbackRos, this, + ros::TransportHints().tcpNoDelay()); // Configure jump warning message if (params_.fp_output.cov_warning) { @@ -405,20 +405,20 @@ void FixpositionDriverNode::PublishNmea() { } } -void FixpositionDriverNode::WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg) { +void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg) { std::unordered_map>> measurements; for (const auto& sensor : msg->sensors) { measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx}); measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy}); measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz}); } - FixpositionDriver::WsCallback(measurements); + WsCallback(measurements); } -void FixpositionDriverNode::RtcmCallback(const fixposition_driver_ros1::RTCMConstPtr& msg) { +void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg) { const void* rtcm_msg = &(msg->message[0]); size_t msg_size = msg->message.size(); - FixpositionDriver::RtcmCallback(rtcm_msg, msg_size); + RtcmCallback(rtcm_msg, msg_size); } void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index df5cd69..96205b0 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -23,12 +23,12 @@ find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(fixposition_driver_lib REQUIRED) +find_package(rtcm_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/Speed.msg msg/Gnsssats.msg msg/NMEA.msg - msg/RTCM.msg msg/WheelSensor.msg msg/fpa/GNSSANT.msg msg/fpa/GNSSCORR.msg @@ -63,6 +63,7 @@ include_directories( include ${sensor_msgs_INCLUDE_DIR} ${fixposition_driver_lib_INCLUDE_DIR} + ${rtcm_msgs_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} ) @@ -86,6 +87,7 @@ endif() target_link_libraries( ${PROJECT_NAME}_exec ${fixposition_driver_lib_LIBRARIES} + ${rtcm_msgs_LIBRARIES} ${Boost_LIBRARIES} ${EIGEN3_LIBRARIES} ${cpp_typesupport_target} @@ -107,7 +109,7 @@ install(DIRECTORY "launch" DESTINATION share/${PROJECT_NAME}/ ) -ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_driver_lib tf2_geometry_msgs) +ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_driver_lib rtcm_msgs tf2_geometry_msgs) # define ament package for this project ament_package() diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 41e2042..adf480a 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -43,9 +43,9 @@ class FixpositionDriverNode : public FixpositionDriver { void RegisterObservers(); - void WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg); + void WsCallbackRos(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg); - void RtcmCallback(const fixposition_driver_ros2::msg::RTCM::ConstSharedPtr msg); + void RtcmCallbackRos(const rtcm_msgs::msg::Message::ConstSharedPtr msg); private: /** @@ -68,7 +68,7 @@ class FixpositionDriverNode : public FixpositionDriver { // ROS subscribers rclcpp::Subscription::SharedPtr ws_sub_; //!< wheelspeed message subscriber - rclcpp::Subscription::SharedPtr rtcm_sub_; //!< RTCM3 message subscriber + rclcpp::Subscription::SharedPtr rtcm_sub_; //!< RTCM3 message subscriber // ROS publishers // FP_A messages diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp index da77bcd..32347f5 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -30,7 +30,9 @@ #include #include #include -#include + +// Include RTCM +#include // Include extras #include diff --git a/fixposition_driver_ros2/launch/serial.yaml b/fixposition_driver_ros2/launch/serial.yaml index 94a8d8f..3692448 100644 --- a/fixposition_driver_ros2/launch/serial.yaml +++ b/fixposition_driver_ros2/launch/serial.yaml @@ -16,4 +16,4 @@ fixposition_driver_ros2: customer_input: speed_topic: "/fixposition/speed" - rtcm_topic: "/fixposition/rtcm" + rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml index e931af5..5955c72 100644 --- a/fixposition_driver_ros2/launch/tcp.yaml +++ b/fixposition_driver_ros2/launch/tcp.yaml @@ -17,4 +17,4 @@ fixposition_driver_ros2: customer_input: speed_topic: "/fixposition/speed" - rtcm_topic: "/fixposition/rtcm" + rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros2/msg/RTCM.msg b/fixposition_driver_ros2/msg/RTCM.msg deleted file mode 100644 index b63c536..0000000 --- a/fixposition_driver_ros2/msg/RTCM.msg +++ /dev/null @@ -1,16 +0,0 @@ -#################################################################################################### -# -# Copyright (c) 2023 -# Fixposition AG -# -#################################################################################################### -# -# Fixposition RTCM Message. -# -# -#################################################################################################### - -# Format | Field | Unit | Description -# --------------|---------------|---------|------------------------------------| -std_msgs/Header header # [-] | Message header. -uint8[] message # [-] | RTCM message. diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml index 43b47df..bf40bcf 100644 --- a/fixposition_driver_ros2/package.xml +++ b/fixposition_driver_ros2/package.xml @@ -24,6 +24,7 @@ tf2_eigen tf2_ros tf2_geometry_msgs + rtcm_msgs rclcpp rosidl_default_runtime diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index ece6d8b..b35c2d2 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -72,10 +72,10 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, { ws_sub_ = node_->create_subscription( params_.customer_input.speed_topic, 100, - std::bind(&FixpositionDriverNode::WsCallback, this, std::placeholders::_1)); - rtcm_sub_ = node_->create_subscription( + std::bind(&FixpositionDriverNode::WsCallbackRos, this, std::placeholders::_1)); + rtcm_sub_ = node_->create_subscription( params_.customer_input.rtcm_topic, 10, - std::bind(&FixpositionDriverNode::RtcmCallback, this, std::placeholders::_1)); + std::bind(&FixpositionDriverNode::RtcmCallbackRos, this, std::placeholders::_1)); // Configure jump warning message if (params_.fp_output.cov_warning) { @@ -413,20 +413,20 @@ void FixpositionDriverNode::PublishNmea() { } } -void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) { +void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) { std::unordered_map>> measurements; for (const auto& sensor : msg->sensors) { measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx}); measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy}); measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz}); } - FixpositionDriver::WsCallback(measurements); + WsCallback(measurements); } -void FixpositionDriverNode::RtcmCallback(const fixposition_driver_ros2::msg::RTCM::ConstSharedPtr msg) { +void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::msg::Message::ConstSharedPtr msg) { const void* rtcm_msg = &(msg->message[0]); size_t msg_size = msg->message.size(); - FixpositionDriver::RtcmCallback(rtcm_msg, msg_size); + RtcmCallback(rtcm_msg, msg_size); } void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, diff --git a/rtcm_msgs b/rtcm_msgs new file mode 160000 index 0000000..8d27ab5 --- /dev/null +++ b/rtcm_msgs @@ -0,0 +1 @@ +Subproject commit 8d27ab5f4d4a581e1bb9c87d572994c25197f6d4