-devel/
+install/
logs/
build/
bin/
# Catkin custom files
CATKIN_IGNORE
+
+# Colcon custom files
+COLCON_IGNORE
-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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+ $<INSTALL_INTERFACE:include>)
+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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+ $<INSTALL_INTERFACE:include>)
+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()
/******************************************************************************
- ROS arc_odometry Package
+ ROS 2 arc_odometry Package
Differential Drive Odometry Class
- Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2018..2022 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
-#ifndef DIFF_ODOMETRY_H
-#define DIFF_ODOMETRY_H
+#ifndef ARC_ODOMETRY__DIFF_ODOMETRY_HPP_
+#define ARC_ODOMETRY__DIFF_ODOMETRY_HPP_
#include <vector>
#include <Eigen/Dense>
-#include <ros/time.h>
+#include <rclcpp/duration.hpp>
+
+#include <arc_odometry/visibility_control.h>
namespace arc_odometry
{
class DiffOdometry
{
public:
+ ARC_ODOMETRY_PUBLIC
DiffOdometry(double wheelBase,std::vector<double> 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<double> wheelRadius);
+
+ ARC_ODOMETRY_PUBLIC
void setPose(const Eigen::Vector3d &x) {x_=x;}
private:
--- /dev/null
+#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_
<?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>arc_odometry</name>
- <version>0.1.0</version>
- <description>The arc_odometry package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <version>2.0.0</version>
+ <description>Computes odometry through circunference arc approximation</description>
<maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>GPLv3</license>
+ <buildtool_depend>ament_cmake_ros</buildtool_depend>
- <!-- Url tags are optional, but multiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/arc_odometry</url> -->
-
+ <depend>rclcpp</depend>
+ <depend>nav_msgs</depend>
+ <depend>sensor_msgs</depend>
+ <depend>tf2_ros</depend>
+ <depend>Eigen3</depend>
- <!-- Author tags are optional, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
-
- <!-- The *depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>Eigen3</build_depend>
- <build_depend>nav_msgs</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>sensor_msgs</build_depend>
- <build_depend>tf</build_depend>
- <build_export_depend>Eigen3</build_export_depend>
- <build_export_depend>nav_msgs</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>sensor_msgs</build_export_depend>
- <build_export_depend>tf</build_export_depend>
- <exec_depend>Eigen3</exec_depend>
- <exec_depend>nav_msgs</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>sensor_msgs</exec_depend>
- <exec_depend>tf</exec_depend>
-
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- Other tools can request additional information be placed here -->
-
+ <build_type>ament_cmake</build_type>
</export>
</package>
/******************************************************************************
- ROS arc_odometry Package
+ ROS 2 arc_odometry Package
Differential Odometry Class
- Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2018..2022 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
-#include <arc_odometry/diff_odometry.h>
+#include <arc_odometry/diff_odometry.hpp>
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<double>::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<double>::epsilon()) u_ << deltaS/dt , deltaU[1]/dt;
Eigen::Vector3d deltaX;
deltaX << deltaS*cos(x_[2]+deltaU[1]/2.0),
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;
/******************************************************************************
- ROS arc_odometry Package
+ ROS 2 arc_odometry Package
Odometry Publisher Node
- Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2018..2022 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
-#include <boost/assign.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_broadcaster.h>
-#include <ros/node_handle.h>
-#include <nav_msgs/Odometry.h>
-#include <sensor_msgs/JointState.h>
-#include <tf/tfMessage.h>
-#include <tf/transform_datatypes.h>
+#include <arc_odometry/diff_odometry.hpp>
-#include <arc_odometry/diff_odometry.h>
-
-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<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
+ std::unique_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
- ros::Subscriber jointStateSub_;
+ rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStateSub_;
std::vector<double> wheelRadius_;
double wheelBase_;
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;
}
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<nav_msgs::Odometry>("odom",100);
-
- tfPub_=node_.advertise<tf::tfMessage>("/tf",100);
+ odomPub_=create_publisher<nav_msgs::msg::Odometry>("odom",100);
- lastSamplingTime_=ros::Time::now();
+ tfBroadcaster_=std::make_unique<tf2_ros::TransformBroadcaster>(*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<sensor_msgs::msg::JointState>("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
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_;
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]);
//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<OdometryPublisher>());
+ rclcpp::shutdown();
return 0;
}
projects / arc_odometry.git / commitdiff