From: Walter Fetter Lages Date: Thu, 17 Mar 2022 20:04:35 +0000 (-0300) Subject: Port to Galactic. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=bb0b0441e069e938a5b50815ff55bc9996593124;p=arc_odometry.git Port to Galactic. --- diff --git a/.gitignore b/.gitignore index 35d74bb..a6b6ae8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -devel/ +install/ logs/ build/ bin/ @@ -49,3 +49,6 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE + +# Colcon custom files +COLCON_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt index d2d1bd2..416f08d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,211 +1,79 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(arc_odometry) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - nav_msgs - roscpp - sensor_msgs - tf -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(Eigen3 REQUIRED) - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# nav_msgs# sensor_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES arc_odometry - CATKIN_DEPENDS nav_msgs roscpp sensor_msgs tf - DEPENDS EIGEN3 +add_library(arc_odometry src/diff_odometry.cpp) +target_compile_features(arc_odometry PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(arc_odometry PUBLIC + $ + $ + $) +ament_target_dependencies( + arc_odometry + "rclcpp" + "nav_msgs" + "sensor_msgs" + "tf2_ros" + "Eigen3" ) -########### -## Build ## -########### +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(arc_odometry PRIVATE "ARC_ODOMETRY_BUILDING_LIBRARY") -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -# TODO: Check names of system library include directories (Eigen3) - ${EIGEN3_INCLUDE_DIRS} +install( + DIRECTORY include/ + DESTINATION include ) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/diff_odometry.cpp +install( + TARGETS arc_odometry + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide add_executable(odometry_publisher src/odometry_publisher.cpp) +target_include_directories(odometry_publisher PUBLIC + $ + $ + $) +target_link_libraries(odometry_publisher arc_odometry) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(odometry_publisher ${PROJECT_NAME} - ${catkin_LIBRARIES} -# ${Eigen3_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html install(TARGETS odometry_publisher - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +ament_export_libraries( + arc_odometry ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +ament_export_targets( + export_${PROJECT_NAME} ) -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_arc_odometry.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/include/arc_odometry/diff_odometry.h b/include/arc_odometry/diff_odometry.hpp similarity index 69% rename from include/arc_odometry/diff_odometry.h rename to include/arc_odometry/diff_odometry.hpp index eefa29a..c30f06e 100644 --- a/include/arc_odometry/diff_odometry.h +++ b/include/arc_odometry/diff_odometry.hpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS arc_odometry Package + ROS 2 arc_odometry Package Differential Drive Odometry Class - Copyright (C) 2018 Walter Fetter Lages + Copyright (C) 2018..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,33 +19,59 @@ *******************************************************************************/ -#ifndef DIFF_ODOMETRY_H -#define DIFF_ODOMETRY_H +#ifndef ARC_ODOMETRY__DIFF_ODOMETRY_HPP_ +#define ARC_ODOMETRY__DIFF_ODOMETRY_HPP_ #include #include -#include +#include + +#include namespace arc_odometry { class DiffOdometry { public: + ARC_ODOMETRY_PUBLIC DiffOdometry(double wheelBase,std::vector wheelRadius); + + ARC_ODOMETRY_PUBLIC ~DiffOdometry(void); - - void update(double leftDisp,double rightDisp,const ros::Duration &duration); - void update(const Eigen::Vector2d &deltaPhi,const ros::Duration &duration); + + ARC_ODOMETRY_PUBLIC + void update(double leftDisp,double rightDisp,const rclcpp::Duration &duration); + + ARC_ODOMETRY_PUBLIC + void update(const Eigen::Vector2d &deltaPhi,const rclcpp::Duration &duration); + + ARC_ODOMETRY_PUBLIC double x(void) const {return x_[0];} + + ARC_ODOMETRY_PUBLIC double y(void) const {return x_[1];} + + ARC_ODOMETRY_PUBLIC double heading(void) const {return x_[2];} + + ARC_ODOMETRY_PUBLIC void getPose(Eigen::Vector3d &x) const {x=x_;} + + ARC_ODOMETRY_PUBLIC double linear(void) const {return u_[0];} + + ARC_ODOMETRY_PUBLIC double angular(void) const {return u_[1];} + + ARC_ODOMETRY_PUBLIC void getVelocity(Eigen::Vector2d &u) const {u=u_;} + + ARC_ODOMETRY_PUBLIC void setParams(double wheelBase,std::vector wheelRadius); + + ARC_ODOMETRY_PUBLIC void setPose(const Eigen::Vector3d &x) {x_=x;} private: diff --git a/include/arc_odometry/visibility_control.h b/include/arc_odometry/visibility_control.h new file mode 100644 index 0000000..462e304 --- /dev/null +++ b/include/arc_odometry/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef ARC_ODOMETRY__VISIBILITY_CONTROL_H_ +#define ARC_ODOMETRY__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ARC_ODOMETRY_EXPORT __attribute__ ((dllexport)) + #define ARC_ODOMETRY_IMPORT __attribute__ ((dllimport)) + #else + #define ARC_ODOMETRY_EXPORT __declspec(dllexport) + #define ARC_ODOMETRY_IMPORT __declspec(dllimport) + #endif + #ifdef ARC_ODOMETRY_BUILDING_LIBRARY + #define ARC_ODOMETRY_PUBLIC ARC_ODOMETRY_EXPORT + #else + #define ARC_ODOMETRY_PUBLIC ARC_ODOMETRY_IMPORT + #endif + #define ARC_ODOMETRY_PUBLIC_TYPE ARC_ODOMETRY_PUBLIC + #define ARC_ODOMETRY_LOCAL +#else + #define ARC_ODOMETRY_EXPORT __attribute__ ((visibility("default"))) + #define ARC_ODOMETRY_IMPORT + #if __GNUC__ >= 4 + #define ARC_ODOMETRY_PUBLIC __attribute__ ((visibility("default"))) + #define ARC_ODOMETRY_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ARC_ODOMETRY_PUBLIC + #define ARC_ODOMETRY_LOCAL + #endif + #define ARC_ODOMETRY_PUBLIC_TYPE +#endif + +#endif // ARC_ODOMETRY__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml index f65e87f..e2ee8e5 100644 --- a/package.xml +++ b/package.xml @@ -1,75 +1,24 @@ - + + arc_odometry - 0.1.0 - The arc_odometry package - - - - + 2.0.0 + Computes odometry through circunference arc approximation Walter Fetter Lages - - - - - GPLv3 + ament_cmake_ros - - - - - + rclcpp + nav_msgs + sensor_msgs + tf2_ros + Eigen3 - - - - - Walter Fetter Lages + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - Eigen3 - nav_msgs - roscpp - sensor_msgs - tf - Eigen3 - nav_msgs - roscpp - sensor_msgs - tf - Eigen3 - nav_msgs - roscpp - sensor_msgs - tf - - - - - + ament_cmake diff --git a/src/diff_odometry.cpp b/src/diff_odometry.cpp index 13b3241..12ef730 100644 --- a/src/diff_odometry.cpp +++ b/src/diff_odometry.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS arc_odometry Package + ROS 2 arc_odometry Package Differential Odometry Class - Copyright (C) 2018 Walter Fetter Lages + Copyright (C) 2018..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,7 +19,7 @@ *******************************************************************************/ -#include +#include namespace arc_odometry { @@ -40,18 +40,18 @@ namespace arc_odometry // not velocity // deltaPhi[0] is the left wheel angular displacement // deltaPhi[1] is the right wheel angular displacement - void DiffOdometry::update(const Eigen::Vector2d &deltaPhi,const ros::Duration &duration) + void DiffOdometry::update(const Eigen::Vector2d &deltaPhi,const rclcpp::Duration &duration) { - double dt=duration.toSec(); + double dt=duration.nanoseconds()*1e9; Eigen::Vector2d deltaU; deltaU << (deltaPhi[0]*wheelRadius_[0]+deltaPhi[1]*wheelRadius_[1])/2.0, (deltaPhi[1]*wheelRadius_[1]-deltaPhi[0]*wheelRadius_[0])/wheelBase_; - double deltaS=(fabs(deltaU[1]) > DBL_EPSILON)? + double deltaS=(fabs(deltaU[1]) > std::numeric_limits::epsilon())? deltaU[0]*sin(deltaU[1]/2)/(deltaU[1]/2):deltaU[0]; - if(dt > DBL_EPSILON) u_ << deltaS/dt , deltaU[1]/dt; + if(dt > std::numeric_limits::epsilon()) u_ << deltaS/dt , deltaU[1]/dt; Eigen::Vector3d deltaX; deltaX << deltaS*cos(x_[2]+deltaU[1]/2.0), @@ -61,7 +61,7 @@ namespace arc_odometry x_+=deltaX; } - void DiffOdometry::update(double leftDisp,double rightDisp,const ros::Duration &duration) + void DiffOdometry::update(double leftDisp,double rightDisp,const rclcpp::Duration &duration) { Eigen::Vector2d deltaPhi; deltaPhi << leftDisp, rightDisp; diff --git a/src/odometry_publisher.cpp b/src/odometry_publisher.cpp index 83c3301..6815a72 100644 --- a/src/odometry_publisher.cpp +++ b/src/odometry_publisher.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS arc_odometry Package + ROS 2 arc_odometry Package Odometry Publisher Node - Copyright (C) 2018 Walter Fetter Lages + Copyright (C) 2018..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,32 +19,28 @@ *******************************************************************************/ -#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include +#include -#include - -class OdometryPublisher +class OdometryPublisher: public rclcpp::Node { public: - OdometryPublisher(ros::NodeHandle node); - ~OdometryPublisher(void); - void publish(void) const; + OdometryPublisher(void); + void publish(void); private: - ros::NodeHandle node_; - Eigen::Vector2d phi_; - ros::Publisher odomPub_; - ros::Publisher tfPub_; + rclcpp::Publisher::SharedPtr odomPub_; + std::unique_ptr tfBroadcaster_; - ros::Subscriber jointStateSub_; + rclcpp::Subscription::SharedPtr jointStateSub_; std::vector wheelRadius_; double wheelBase_; @@ -54,26 +50,26 @@ class OdometryPublisher std::string odom_frame_id_; std::string base_frame_id_; - ros::Time lastSamplingTime_; + rclcpp::Time lastSamplingTime_; + rclcpp::TimerBase::SharedPtr timer_; - void jointStateCB(const sensor_msgs::JointState::ConstPtr &jointState); + void jointStateCB(const sensor_msgs::msg::JointState::SharedPtr jointState); }; -OdometryPublisher::OdometryPublisher(ros::NodeHandle node): +OdometryPublisher::OdometryPublisher(void): Node("odometry_publisher"), wheelRadius_(2),odom_(wheelBase_,wheelRadius_) { - node_=node; - - ros::NodeHandle n("~"); - if(!n.getParam("wheel_separation",wheelBase_)) + declare_parameter("wheel_separation",rclcpp::PARAMETER_DOUBLE); + if(!get_parameter("wheel_separation",wheelBase_)) { - ROS_ERROR("No 'wheel_separation' in node %s.",node_.getNamespace().c_str()); + RCLCPP_ERROR_STREAM(get_logger(),"No 'wheel_separation' parameter in node " << get_fully_qualified_name() << "."); return; } - - if(!n.getParam("wheel_radius",wheelRadius_)) + + declare_parameter("wheel_radius",rclcpp::PARAMETER_DOUBLE_ARRAY); + if(!get_parameter("wheel_radius",wheelRadius_)) { - ROS_ERROR("No 'wheel_radius' in node %s.",node_.getNamespace().c_str()); + RCLCPP_ERROR_STREAM(get_logger(),"No 'wheel_radius' parameter in node " << get_fully_qualified_name() << "."); return; } @@ -81,30 +77,32 @@ OdometryPublisher::OdometryPublisher(ros::NodeHandle node): phi_.setZero(); odom_frame_id_="odom"; - n.param("odom_frame_id",odom_frame_id_,odom_frame_id_); + declare_parameter("odom_frame_id",odom_frame_id_); + get_parameter("odom_frame_id",odom_frame_id_); base_frame_id_="base_link"; - n.param("base_frame_id",base_frame_id_,base_frame_id_); + declare_parameter("base_frame_id",base_frame_id_); + get_parameter("base_frame_id",base_frame_id_); - odomPub_=node_.advertise("odom",100); - - tfPub_=node_.advertise("/tf",100); + odomPub_=create_publisher("odom",100); - lastSamplingTime_=ros::Time::now(); + tfBroadcaster_=std::make_unique(*this); - jointStateSub_=node_.subscribe("joint_states",100,&OdometryPublisher::jointStateCB,this); -} + lastSamplingTime_=this->get_clock()->now(); -OdometryPublisher::~OdometryPublisher(void) -{ - tfPub_.shutdown(); - odomPub_.shutdown(); - jointStateSub_.shutdown(); + using std::placeholders::_1; + jointStateSub_=create_subscription("joint_states",100,std::bind(&OdometryPublisher::jointStateCB,this,_1)); + + int rate=100; + declare_parameter("publish_rate",rate); + get_parameter("publish_rate",rate); + + timer_=rclcpp::create_timer(this,this->get_clock(),rclcpp::Duration::from_seconds(1.0/rate),std::bind(&OdometryPublisher::publish,this)); } -void OdometryPublisher::jointStateCB(const sensor_msgs::JointState::ConstPtr &jointState) +void OdometryPublisher::jointStateCB(const sensor_msgs::msg::JointState::SharedPtr jointState) { - ros::Time time=jointState->header.stamp; + rclcpp::Time time=jointState->header.stamp; // Incremental encoders sense angular displacement and // not velocity @@ -121,16 +119,16 @@ void OdometryPublisher::jointStateCB(const sensor_msgs::JointState::ConstPtr &jo lastSamplingTime_=time; } -void OdometryPublisher::publish(void) const +void OdometryPublisher::publish(void) { - ros::Time time=ros::Time::now(); + rclcpp::Time time=this->get_clock()->now(); Eigen::Vector3d x; Eigen::Vector2d u; odom_.getPose(x); odom_.getVelocity(u); - nav_msgs::Odometry odomMsg; + nav_msgs::msg::Odometry odomMsg; odomMsg.header.stamp=time; odomMsg.header.frame_id=odom_frame_id_; odomMsg.child_frame_id=base_frame_id_; @@ -138,19 +136,18 @@ void OdometryPublisher::publish(void) const odomMsg.pose.pose.position.x=x[0]; odomMsg.pose.pose.position.y=x[1]; odomMsg.pose.pose.position.z=0; - - odomMsg.pose.pose.orientation=tf::createQuaternionMsgFromYaw(x[2]); + + odomMsg.pose.pose.orientation=tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0,0.0,1.0),x[2])); // Fake covariance double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; - odomMsg.pose.covariance=boost::assign::list_of - (pose_cov[0]) (0) (0) (0) (0) (0) - (0) (pose_cov[1]) (0) (0) (0) (0) - (0) (0) (pose_cov[2]) (0) (0) (0) - (0) (0) (0) (pose_cov[3]) (0) (0) - (0) (0) (0) (0) (pose_cov[4]) (0) - (0) (0) (0) (0) (0) (pose_cov[5]); - + odomMsg.pose.covariance={ + pose_cov[0], 0, 0, 0, 0, 0, + 0, pose_cov[1], 0, 0, 0, 0, + 0, 0, pose_cov[2], 0, 0, 0, + 0, 0, 0, pose_cov[3], 0, 0, + 0, 0, 0, 0, pose_cov[4], 0, + 0, 0, 0, 0, 0, pose_cov[5]}; odomMsg.twist.twist.linear.x=u[0]*cos(x[2]); odomMsg.twist.twist.linear.y=u[0]*sin(x[2]); @@ -161,50 +158,29 @@ void OdometryPublisher::publish(void) const //Fake covariance double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; - odomMsg.twist.covariance=boost::assign::list_of - (twist_cov[0]) (0) (0) (0) (0) (0) - (0) (twist_cov[1]) (0) (0) (0) (0) - (0) (0) (twist_cov[2]) (0) (0) (0) - (0) (0) (0) (twist_cov[3]) (0) (0) - (0) (0) (0) (0) (twist_cov[4]) (0) - (0) (0) (0) (0) (0) (twist_cov[5]); - - - odomPub_.publish(odomMsg); - - tf::tfMessage tfMsg; - tfMsg.transforms.resize(1); - tfMsg.transforms[0].transform.translation.z=0.0; - tfMsg.transforms[0].child_frame_id=base_frame_id_; - tfMsg.transforms[0].header.frame_id=odom_frame_id_; - - geometry_msgs::TransformStamped &odom_frame=tfMsg.transforms[0]; - odom_frame.header.stamp=time; - odom_frame.transform.translation.x=x[0]; - odom_frame.transform.translation.y=x[1]; - odom_frame.transform.rotation=tf::createQuaternionMsgFromYaw(x[2]); + odomMsg.twist.covariance={ + twist_cov[0], 0, 0, 0, 0, 0, + 0, twist_cov[1], 0, 0, 0, 0, + 0, 0, twist_cov[2], 0, 0, 0, + 0, 0, 0, twist_cov[3], 0, 0, + 0, 0, 0, 0, twist_cov[4], 0, + 0, 0, 0, 0, 0, twist_cov[5]}; + + odomPub_->publish(odomMsg); + + geometry_msgs::msg::TransformStamped odomFrame; + odomFrame.header.stamp=time; + odomFrame.transform.translation.x=x[0]; + odomFrame.transform.translation.y=x[1]; + odomFrame.transform.rotation=tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0,0.0,1.0),x[2])); - tfPub_.publish(tfMsg); + tfBroadcaster_->sendTransform(odomFrame); } int main(int argc,char* argv[]) { - ros::init(argc,argv,"odometry_publisher"); - ros::NodeHandle node; - - OdometryPublisher odomPublisher(node); - - int rate=100; - ros::NodeHandle n("~"); - n.param("publish_rate",rate,rate); - - ros::Rate loop(rate); - while(ros::ok()) - { - odomPublisher.publish(); - - ros::spinOnce(); - loop.sleep(); - } + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; }