diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp index 88333d5..9d1bc80 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp @@ -149,6 +149,8 @@ class FixpositionDriver { int client_fd_ = -1; //!< TCP or Serial file descriptor int connection_status_ = -1; struct termios options_save_; + uint8_t readBuf[8192]; + int buf_size = 0; }; } // namespace fixposition #endif //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp index 2fd0b55..03f5b7d 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp @@ -30,6 +30,7 @@ struct FpOutputParams { std::vector formats; //!< data formats to convert, supports "FP" for now std::string qos_type; //!< ROS QoS type, supports "sensor_" and "default_" bool cov_warning; //!< enable/disable covariance warning + bool nav2_mode; //!< enable/disable nav2 mode std::string ip; //!< IP address for TCP connection std::string port; //!< Port for TCP connection diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 92da33d..39dd7a4 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -257,66 +257,67 @@ bool FixpositionDriver::RunOnce() { } bool FixpositionDriver::ReadAndPublish() { - char readBuf[8192]; - - ssize_t rv; - if (params_.fp_output.type == INPUT_TYPE::TCP) { - rv = recv(client_fd_, (void*)&readBuf, sizeof(readBuf), MSG_DONTWAIT); - } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) { - rv = read(client_fd_, (void*)&readBuf, sizeof(readBuf)); - } else { - rv = 0; - } - - if (rv == 0) { - std::cerr << "Connection closed.\n"; - return false; - } - - if (rv < 0 && errno == EAGAIN) { - /* no data for now, call back when the socket is readable */ + int msg_size = 0; + // Nov B + msg_size = IsNovMessage(readBuf, buf_size); + if (msg_size > 0) { + NovConvertAndPublish(readBuf); + buf_size -= msg_size; + if (buf_size > 0) { + memmove(readBuf, &readBuf[msg_size], buf_size); + } return true; - } - if (rv < 0) { - std::cerr << "Connection error.\n"; - return false; - } - - ssize_t start_id = 0; - while (start_id < rv) { - int msg_size = 0; - // Nov B - msg_size = IsNovMessage((uint8_t*)&readBuf[start_id], rv - start_id); + } else if (msg_size == 0) { + // Nmea (incl. FP_A) + msg_size = IsNmeaMessage((char*)readBuf, buf_size); if (msg_size > 0) { - NovConvertAndPublish((uint8_t*)&readBuf[start_id]); - start_id += msg_size; - continue; - } - if (msg_size == 0) { - // do nothing - } - if (msg_size < 0) { - break; + NmeaConvertAndPublish({(const char*)readBuf, (const char*)readBuf + msg_size}); + buf_size -= msg_size; + if (buf_size > 0) { + memmove(readBuf, &readBuf[msg_size], buf_size); + } + return true; + } else if (msg_size == 0) { + // If not NOV_B nor NMEA, remove 1 char + if (buf_size > 0) { + buf_size -= 1; + memmove(readBuf, &readBuf[1], buf_size); + } + } else { + // Wait for more data } + } else { + // wait for more data + } - // Nmea (incl. FP_A) - msg_size = IsNmeaMessage(&readBuf[start_id], rv - start_id); - if (msg_size > 0) { - // NovConvertAndPublish(start, msg_size); - std::string msg(&readBuf[start_id], msg_size); - NmeaConvertAndPublish(msg); - start_id += msg_size; - continue; + // Read more data from the TCP/Serial port + int rem_size = sizeof(readBuf) - buf_size; + if (rem_size > 0) { + ssize_t rv; + if (params_.fp_output.type == INPUT_TYPE::TCP) { + rv = recv(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size, MSG_DONTWAIT); + } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) { + rv = read(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size); + } else { + rv = 0; } - if (msg_size == 0) { - // do nothing + + if (rv == 0) { + std::cerr << "Connection closed.\n"; + return false; } - if (msg_size < 0) { - break; + + if (rv < 0 && errno == EAGAIN) { + /* no data for now, call back when the socket is readable */ + return true; + } + + if (rv < 0) { + std::cerr << "Connection error.\n"; + return false; } - // No Match, increment by 1 - ++start_id; + buf_size += rv; } return true; diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 339805b..a002a20 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation eigen_conversions + tf2_ros + tf2_geometry_msgs ) add_message_files( FILES @@ -61,8 +63,8 @@ generate_messages( catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS - tf - roscpp geometry_msgs + tf tf2_ros + roscpp geometry_msgs tf2_geometry_msgs nav_msgs std_msgs message_runtime ) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index 1ad082b..9872d38 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -94,6 +94,23 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWit */ void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub); +/** + * @brief + * + * @param[in] data + * @param[out] tf + */ +void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf); + +/** + * @brief + * + * @param[in] tf_map + * @param[out] static_br_ + * @param[out] br_ + */ +void PublishNav2Tf(const std::map>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_); + /** * @brief * 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 87dd17f..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 std_msgs::UInt8MultiArray::ConstPtr& msg); + void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg); private: /** @@ -126,6 +126,14 @@ class FixpositionDriverNode : public FixpositionDriver { // Previous state Eigen::Vector3d prev_pos; Eigen::MatrixXd prev_cov; + + // Nav2 TF map + std::map> tf_map = { + {"ECEFENU0", nullptr}, + {"POIPOISH", nullptr}, + {"ECEFPOISH", nullptr}, + {"ENU0POI", nullptr} + }; }; } // namespace fixposition 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 89a7356..03c5e3d 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp @@ -22,12 +22,17 @@ #include #include #include +#include +#include // Include generic #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 d077a1d..62f9ff0 100644 --- a/fixposition_driver_ros1/launch/serial.yaml +++ b/fixposition_driver_ros1/launch/serial.yaml @@ -10,7 +10,8 @@ fp_output: rate: 200 reconnect_delay: 5.0 # wait time in [s] until retry connection cov_warning: false + nav2_mode: false 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 7073945..1f7745f 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -10,7 +10,8 @@ fp_output: rate: 200 reconnect_delay: 5.0 # wait time in [s] until retry connection cov_warning: false + nav2_mode: false 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 ee29d50..7956e19 100644 --- a/fixposition_driver_ros1/package.xml +++ b/fixposition_driver_ros1/package.xml @@ -18,4 +18,7 @@ message_generation message_runtime fixposition_driver_lib + rtcm_msgs + tf2_ros + tf2_geometry_msgs diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 31b7fa1..7910e8f 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -596,27 +596,87 @@ void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pu return; } - // Create message - geometry_msgs::TransformStamped msg; - // Populate message - msg.header.frame_id = data.odom.frame_id; - msg.child_frame_id = data.odom.child_frame_id; - - if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); - } - - tf::quaternionEigenToMsg(data.odom.pose.orientation, msg.transform.rotation); - tf::vectorEigenToMsg(data.odom.pose.position, msg.transform.translation); + geometry_msgs::TransformStamped msg; + OdomToTf(data.odom, msg); // Publish message pub.sendTransform(msg); } } +void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf) { + // Populate message + tf.header.frame_id = data.frame_id; + tf.child_frame_id = data.child_frame_id; + + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { + tf.header.stamp = ros::Time::now(); + } else { + tf.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp)); + } + + tf::quaternionEigenToMsg(data.pose.orientation, tf.transform.rotation); + tf::vectorEigenToMsg(data.pose.position, tf.transform.translation); +} + +void PublishNav2Tf(const std::map>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_) { + if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) { + // Publish FP_ECEF -> map + tf_map.at("ECEFENU0")->child_frame_id = "map"; + static_br_.sendTransform(*tf_map.at("ECEFENU0")); + + // Compute FP_ENU0 -> FP_POISH + // Extract translation and rotation from ECEFENU0 + geometry_msgs::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation; + geometry_msgs::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation; + Eigen::Vector3d t_ecef_enu0_; + t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z; + Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z); + + // Extract translation and rotation from ECEFPOISH + geometry_msgs::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation; + geometry_msgs::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation; + Eigen::Vector3d t_ecef_poish; + t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z; + Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z); + + // Compute the ENU transformation + const Eigen::Vector3d t_enu0_poish = fixposition::TfEnuEcef(t_ecef_poish, fixposition::TfWgs84LlhEcef(t_ecef_enu0_)); + const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish; + + // Create tf2::Transform tf_ENU0POISH + tf2::Transform tf_ENU0POISH; + tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z())); + tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w()); + tf_ENU0POISH.setRotation(tf_q_enu0_poish); + + // Publish map -> odom + // Multiply the transforms + tf2::Transform tf_ENU0POI; + tf2::fromMsg(tf_map.at("ENU0POI")->transform, tf_ENU0POI); + tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse(); + + // Create a new TransformStamped message + geometry_msgs::TransformStamped tf_map_odom; + tf_map_odom.header.stamp = ros::Time::now(); + tf_map_odom.header.frame_id = "map"; + tf_map_odom.child_frame_id = "odom"; + tf_map_odom.transform = tf2::toMsg(tf_combined); + br_.sendTransform(tf_map_odom); + + // Publish odom -> base_link + geometry_msgs::TransformStamped tf_odom_base; + tf_odom_base.header.stamp = ros::Time::now(); + tf_odom_base.header.frame_id = "odom"; + tf_odom_base.child_frame_id = "base_link"; + tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH); + + // Send the transform + br_.sendTransform(tf_odom_base); + } +} + void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { // Create message diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index f6ab947..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) { @@ -121,7 +121,7 @@ void FixpositionDriverNode::RegisterObservers() { if (!prev_pos.isZero() && !prev_cov.isZero()) { Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs(); - if ((pos_diff[0] > 0) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) { + if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) { JumpWarningMsg(data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_); } } @@ -135,12 +135,28 @@ void FixpositionDriverNode::RegisterObservers() { FpToRosMsg(data, fpa_odomenu_pub_); FpToRosMsg(data.odom, odometry_enu_pub_); OdomToYprMsg(data.odom, eul_pub_); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_ENU0 -> FP_POI + geometry_msgs::TransformStamped tf; + OdomToTf(data.odom, tf); + tf_map["ENU0POI"] = std::make_shared(tf); + } }); } else if (format == "ODOMSH") { dynamic_cast*>(a_converters_["ODOMSH"].get()) ->AddObserver([this](const FP_ODOMSH& data) { FpToRosMsg(data, fpa_odomsh_pub_); FpToRosMsg(data.odom, odometry_smooth_pub_); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_ECEF -> FP_POISH + geometry_msgs::TransformStamped tf; + OdomToTf(data.odom, tf); + tf_map["ECEFPOISH"] = std::make_shared(tf); + } }); } else if (format == "ODOMSTATUS") { dynamic_cast*>(a_converters_["ODOMSTATUS"].get()) @@ -150,7 +166,14 @@ void FixpositionDriverNode::RegisterObservers() { ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); }); } else if (format == "EOE") { dynamic_cast*>(a_converters_["EOE"].get()) - ->AddObserver([this](const FP_EOE& data) { FpToRosMsg(data, fpa_eoe_pub_); }); + ->AddObserver([this](const FP_EOE& data) { + FpToRosMsg(data, fpa_eoe_pub_); + + // Generate Nav2 TF tree + if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) { + PublishNav2Tf(tf_map, static_br_, br_); + } + }); } else if (format == "LLH") { dynamic_cast*>(a_converters_["LLH"].get()) ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); }); @@ -189,6 +212,20 @@ void FixpositionDriverNode::RegisterObservers() { } else if (tf.child_frame_id == "FP_POISH" && tf.header.frame_id == "FP_POI") { br_.sendTransform(tf); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_POI -> FP_POISH + tf_map["POIPOISH"] = std::make_shared(tf); + } + } else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") { + static_br_.sendTransform(tf); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_ECEF -> FP_ENU0 + tf_map["ECEFENU0"] = std::make_shared(tf); + } } else { static_br_.sendTransform(tf); } @@ -368,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 std_msgs::UInt8MultiArray::ConstPtr& msg) { - const void* rtcm_msg = &(msg->data[0]); - size_t msg_size = msg->layout.dim[0].size; - FixpositionDriver::RtcmCallback(rtcm_msg, msg_size); +void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg) { + const void* rtcm_msg = &(msg->message[0]); + size_t msg_size = msg->message.size(); + RtcmCallback(rtcm_msg, msg_size); } void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp index c611ab9..0337b30 100644 --- a/fixposition_driver_ros1/src/params.cpp +++ b/fixposition_driver_ros1/src/params.cpp @@ -26,6 +26,7 @@ bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params) { const std::string PORT = ns + "/port"; const std::string BAUDRATE = ns + "/baudrate"; const std::string COV_WARNING = ns + "/cov_warning"; + const std::string NAV2_MODE = ns + "/nav2_mode"; // read parameters if (!ros::param::get(RATE, params.rate)) { @@ -40,6 +41,10 @@ bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params) { params.cov_warning = false; ROS_WARN("Using Default Covariance Warning option : %i", params.cov_warning); } + if (!ros::param::get(NAV2_MODE, params.nav2_mode)) { + params.nav2_mode = false; + ROS_WARN("Using Default Nav2 mode option : %i", params.nav2_mode); + } std::string type_str; if (!ros::param::get(TYPE, type_str)) { diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index b8bbc8e..96205b0 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -21,7 +21,9 @@ find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) 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 @@ -61,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} ) @@ -84,6 +87,7 @@ endif() target_link_libraries( ${PROJECT_NAME}_exec ${fixposition_driver_lib_LIBRARIES} + ${rtcm_msgs_LIBRARIES} ${Boost_LIBRARIES} ${EIGEN3_LIBRARIES} ${cpp_typesupport_target} @@ -105,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) +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/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index 21c7943..31959e8 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -115,6 +115,23 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::msg::Twi */ void OdometryDataToTf(const FP_ODOMETRY& data, std::shared_ptr pub); +/** + * @brief + * + * @param[in] data + * @param[out] tf + */ +void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf); + +/** + * @brief + * + * @param[in] tf_map + * @param[out] static_br_ + * @param[out] br_ + */ +void PublishNav2Tf(const std::map>& tf_map, std::shared_ptr static_br_, std::shared_ptr br_); + /** * @brief * 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 453dbdb..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 std_msgs::msg::UInt8MultiArray::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 @@ -126,6 +126,14 @@ class FixpositionDriverNode : public FixpositionDriver { // Previous state Eigen::Vector3d prev_pos; Eigen::MatrixXd prev_cov; + + // Nav2 TF map + std::map> tf_map = { + {"ECEFENU0", nullptr}, + {"POIPOISH", nullptr}, + {"ECEFPOISH", nullptr}, + {"ENU0POI", nullptr} + }; }; } // namespace fixposition 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 2ae8657..32347f5 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp @@ -23,12 +23,17 @@ #include #include #include +#include +#include // Include generic #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 63a7a81..3692448 100644 --- a/fixposition_driver_ros2/launch/serial.yaml +++ b/fixposition_driver_ros2/launch/serial.yaml @@ -12,7 +12,8 @@ fixposition_driver_ros2: rate: 200 reconnect_delay: 5.0 # wait time in [s] until retry connection cov_warning: false + nav2_mode: false 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 b284d5b..5955c72 100644 --- a/fixposition_driver_ros2/launch/tcp.yaml +++ b/fixposition_driver_ros2/launch/tcp.yaml @@ -13,7 +13,8 @@ fixposition_driver_ros2: reconnect_delay: 5.0 # wait time in [s] until retry connection qos_type: "sensor_short" # Supported types: "sensor_short", "sensor_long", "default_short", "default_long" cov_warning: false + nav2_mode: false customer_input: speed_topic: "/fixposition/speed" - rtcm_topic: "/fixposition/rtcm" + rtcm_topic: "/rtcm" diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml index 00c8a89..bf40bcf 100644 --- a/fixposition_driver_ros2/package.xml +++ b/fixposition_driver_ros2/package.xml @@ -23,6 +23,8 @@ tf2 tf2_eigen tf2_ros + tf2_geometry_msgs + rtcm_msgs rclcpp rosidl_default_runtime diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 8411375..28048fb 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -595,24 +595,84 @@ void OdometryDataToTf(const FP_ODOMETRY& data, std::shared_ptrsendTransform(msg); + } +} - if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) { - msg.header.stamp = rclcpp::Clock().now(); - } else { - msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp); - } +void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf) { + // Populate message + tf.header.frame_id = data.frame_id; + tf.child_frame_id = data.child_frame_id; + + if (data.stamp.tow == 0.0 && data.stamp.wno == 0) { + tf.header.stamp = rclcpp::Clock().now(); + } else { + tf.header.stamp = GpsTimeToMsgTime(data.stamp); + } - msg.transform.rotation = tf2::toMsg(data.odom.pose.orientation); - tf2::toMsg(data.odom.pose.position, msg.transform.translation); + tf.transform.rotation = tf2::toMsg(data.pose.orientation); + tf2::toMsg(data.pose.position, tf.transform.translation); +} - // Publish message - pub->sendTransform(msg); +void PublishNav2Tf(const std::map>& tf_map, std::shared_ptr static_br_, std::shared_ptr br_) { + if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) { + // Publish FP_ECEF -> map + tf_map.at("ECEFENU0")->child_frame_id = "map"; + static_br_->sendTransform(*tf_map.at("ECEFENU0")); + + // Compute FP_ENU0 -> FP_POISH + // Extract translation and rotation from ECEFENU0 + geometry_msgs::msg::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation; + geometry_msgs::msg::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation; + Eigen::Vector3d t_ecef_enu0_; + t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z; + Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z); + + // Extract translation and rotation from ECEFPOISH + geometry_msgs::msg::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation; + geometry_msgs::msg::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation; + Eigen::Vector3d t_ecef_poish; + t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z; + Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z); + + // Compute the ENU transformation + const Eigen::Vector3d t_enu0_poish = fixposition::TfEnuEcef(t_ecef_poish, fixposition::TfWgs84LlhEcef(t_ecef_enu0_)); + const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish; + + // Create tf2::Transform tf_ENU0POISH + tf2::Transform tf_ENU0POISH; + tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z())); + tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w()); + tf_ENU0POISH.setRotation(tf_q_enu0_poish); + + // Publish map -> odom + // Multiply the transforms + tf2::Transform tf_ENU0POI; + tf2::fromMsg(tf_map.at("ENU0POI")->transform, tf_ENU0POI); + tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse(); + + // Create a new TransformStamped message + geometry_msgs::msg::TransformStamped tf_map_odom; + tf_map_odom.header.stamp = rclcpp::Clock().now(); + tf_map_odom.header.frame_id = "map"; + tf_map_odom.child_frame_id = "odom"; + tf_map_odom.transform = tf2::toMsg(tf_combined); + br_->sendTransform(tf_map_odom); + + // Publish odom -> base_link + geometry_msgs::msg::TransformStamped tf_odom_base; + tf_odom_base.header.stamp = rclcpp::Clock().now(); + tf_odom_base.header.frame_id = "odom"; + tf_odom_base.child_frame_id = "base_link"; + tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH); + + // Send the transform + br_->sendTransform(tf_odom_base); } } diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 4300d05..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) { @@ -127,7 +127,7 @@ void FixpositionDriverNode::RegisterObservers() { if (!prev_pos.isZero() && !prev_cov.isZero()) { Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs(); - if ((pos_diff[0] > 0) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) { + if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) { JumpWarningMsg(node_, data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_); } } @@ -141,12 +141,28 @@ void FixpositionDriverNode::RegisterObservers() { FpToRosMsg(data, fpa_odomenu_pub_); FpToRosMsg(data.odom, odometry_enu_pub_); OdomToYprMsg(data.odom, eul_pub_); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_ENU0 -> FP_POI + geometry_msgs::msg::TransformStamped tf; + OdomToTf(data.odom, tf); + tf_map["ENU0POI"] = std::make_shared(tf); + } }); } else if (format == "ODOMSH") { dynamic_cast*>(a_converters_["ODOMSH"].get()) ->AddObserver([this](const FP_ODOMSH& data) { FpToRosMsg(data, fpa_odomsh_pub_); FpToRosMsg(data.odom, odometry_smooth_pub_); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_ECEF -> FP_POISH + geometry_msgs::msg::TransformStamped tf; + OdomToTf(data.odom, tf); + tf_map["ECEFPOISH"] = std::make_shared(tf); + } }); } else if (format == "ODOMSTATUS") { dynamic_cast*>(a_converters_["ODOMSTATUS"].get()) @@ -156,7 +172,14 @@ void FixpositionDriverNode::RegisterObservers() { ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); }); } else if (format == "EOE") { dynamic_cast*>(a_converters_["EOE"].get()) - ->AddObserver([this](const FP_EOE& data) { FpToRosMsg(data, fpa_eoe_pub_); }); + ->AddObserver([this](const FP_EOE& data) { + FpToRosMsg(data, fpa_eoe_pub_); + + // Generate Nav2 TF tree + if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) { + PublishNav2Tf(tf_map, static_br_, br_); + } + }); } else if (format == "LLH") { dynamic_cast*>(a_converters_["LLH"].get()) ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); }); @@ -197,6 +220,20 @@ void FixpositionDriverNode::RegisterObservers() { } else if (tf.child_frame_id == "FP_POISH" && tf.header.frame_id == "FP_POI") { br_->sendTransform(tf); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_POI -> FP_POISH + tf_map["POIPOISH"] = std::make_shared(tf); + } + } else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") { + static_br_->sendTransform(tf); + + // Append TF if Nav2 mode is selected + if (params_.fp_output.nav2_mode) { + // Get FP_ECEF -> FP_ENU0 + tf_map["ECEFENU0"] = std::make_shared(tf); + } } else { static_br_->sendTransform(tf); } @@ -376,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 std_msgs::msg::UInt8MultiArray::ConstSharedPtr msg) { - const void* rtcm_msg = &(msg->data[0]); - size_t msg_size = msg->layout.dim[0].size; - FixpositionDriver::RtcmCallback(rtcm_msg, msg_size); +void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::msg::Message::ConstSharedPtr msg) { + const void* rtcm_msg = &(msg->message[0]); + size_t msg_size = msg->message.size(); + RtcmCallback(rtcm_msg, msg_size); } void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, diff --git a/fixposition_driver_ros2/src/params.cpp b/fixposition_driver_ros2/src/params.cpp index 3702ab5..90376aa 100644 --- a/fixposition_driver_ros2/src/params.cpp +++ b/fixposition_driver_ros2/src/params.cpp @@ -26,7 +26,8 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n const std::string IP = ns + ".ip"; const std::string PORT = ns + ".port"; const std::string BAUDRATE = ns + ".baudrate"; - const std::string COV_WARNING = ns + "/cov_warning"; + const std::string COV_WARNING = ns + ".cov_warning"; + const std::string NAV2_MODE = ns + ".nav2_mode"; node->declare_parameter(RATE, 100); node->declare_parameter(RECONNECT_DELAY, 5.0); @@ -37,6 +38,7 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n node->declare_parameter(IP, "127.0.0.1"); node->declare_parameter(BAUDRATE, 115200); node->declare_parameter(COV_WARNING, false); + node->declare_parameter(NAV2_MODE, false); // read parameters if (node->get_parameter(RATE, params.rate)) { RCLCPP_INFO(node->get_logger(), "%s : %d", RATE.c_str(), params.rate); @@ -58,6 +60,11 @@ bool LoadParamsFromRos2(std::shared_ptr node, const std::string& n } else { RCLCPP_WARN(node->get_logger(), "%s : %d", COV_WARNING.c_str(), params.cov_warning); } + if (node->get_parameter(NAV2_MODE, params.nav2_mode)) { + RCLCPP_INFO(node->get_logger(), "%s : %d", NAV2_MODE.c_str(), params.nav2_mode); + } else { + RCLCPP_WARN(node->get_logger(), "%s : %d", NAV2_MODE.c_str(), params.nav2_mode); + } std::string type_str; node->get_parameter(TYPE, type_str); diff --git a/rtcm_msgs/CMakeLists.txt b/rtcm_msgs/CMakeLists.txt new file mode 100644 index 0000000..2529b16 --- /dev/null +++ b/rtcm_msgs/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(rtcm_msgs) + +# ros_environment is needed to have ROS_VERSION +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) + +find_package(std_msgs REQUIRED) + +if(${ROS_VERSION} EQUAL 1) + + find_package(catkin REQUIRED + COMPONENTS + message_generation + ) + + add_message_files(DIRECTORY msg + FILES + Message.msg + ) + + generate_messages(DEPENDENCIES std_msgs) + + catkin_package(CATKIN_DEPENDS message_runtime std_msgs) + +elseif(${ROS_VERSION} EQUAL 2) + find_package(ament_cmake REQUIRED) + find_package(builtin_interfaces REQUIRED) + find_package(rosidl_default_generators REQUIRED) + + rosidl_generate_interfaces(${PROJECT_NAME} ADD_LINTER_TESTS + msg/Message.msg + DEPENDENCIES builtin_interfaces std_msgs + ) + + ament_export_dependencies(rosidl_default_runtime) + ament_package() + +endif() diff --git a/rtcm_msgs/LICENSE b/rtcm_msgs/LICENSE new file mode 100644 index 0000000..26a8265 --- /dev/null +++ b/rtcm_msgs/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2021, Marek Materzok +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/rtcm_msgs/msg/Message.msg b/rtcm_msgs/msg/Message.msg new file mode 100644 index 0000000..6553286 --- /dev/null +++ b/rtcm_msgs/msg/Message.msg @@ -0,0 +1,4 @@ +# A message representing a single RTCM message. +std_msgs/Header header + +uint8[] message diff --git a/rtcm_msgs/package.xml b/rtcm_msgs/package.xml new file mode 100644 index 0000000..ee853c3 --- /dev/null +++ b/rtcm_msgs/package.xml @@ -0,0 +1,35 @@ + + + + rtcm_msgs + 1.1.6 + The rtcm_msgs package contains messages related to data in the RTCM format. + Marek Materzok + BSD + Marek Materzok + + catkin + ament_cmake + rosidl_default_generators + + ros_environment + message_generation + builtin_interfaces + + std_msgs + + message_runtime + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + catkin + ament_cmake + + +