├── package.xml ├── CMakeLists.txt ├── src ├── pose_to_odom.cpp ├── odom_to_imu.cpp ├── pose_to_path.cpp ├── odom_to_pose.cpp └── poseWithCovariance_to_ellipsoid.cpp └── cmake-modules └── FindEigen.cmake /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros_utils 5 | 0.0.1 6 | Some useful nodes to work with ROS 7 | 8 | Thomas Fischer 9 | 10 | GPLv3 11 | 12 | catkin 13 | 14 | roscpp 15 | geometry_msgs 16 | nav_msgs 17 | cmake_modules 18 | 19 | roscpp 20 | geometry_msgs 21 | nav_msgs 22 | cmake_modules 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_utils) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules) 5 | 6 | 7 | find_package(catkin REQUIRED COMPONENTS roscpp cmake_modules geometry_msgs nav_msgs tf2 tf2_ros tf2_geometry_msgs) 8 | catkin_package(CATKIN_DEPENDS roscpp geometry_msgs nav_msgs ) 9 | include_directories(${catkin_INCLUDE_DIRS}) 10 | 11 | ADD_EXECUTABLE(pose_to_path src/pose_to_path.cpp) 12 | TARGET_LINK_LIBRARIES(pose_to_path ${catkin_LIBRARIES}) 13 | 14 | ADD_EXECUTABLE(odom_to_pose src/odom_to_pose.cpp) 15 | TARGET_LINK_LIBRARIES(odom_to_pose ${catkin_LIBRARIES}) 16 | 17 | ADD_EXECUTABLE(odom_to_imu src/odom_to_imu.cpp) 18 | TARGET_LINK_LIBRARIES(odom_to_imu ${catkin_LIBRARIES}) 19 | 20 | ADD_EXECUTABLE(pose_to_odom src/pose_to_odom.cpp) 21 | TARGET_LINK_LIBRARIES(pose_to_odom ${catkin_LIBRARIES}) 22 | 23 | ADD_EXECUTABLE(poseWithCovariance_to_ellipsoid src/poseWithCovariance_to_ellipsoid.cpp) 24 | TARGET_LINK_LIBRARIES(poseWithCovariance_to_ellipsoid ${catkin_LIBRARIES}) 25 | -------------------------------------------------------------------------------- /src/pose_to_odom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class PoseToOdom 6 | { 7 | public: 8 | 9 | PoseToOdom(); 10 | 11 | private: 12 | 13 | ros::NodeHandle nh_, nhp_; 14 | 15 | ros::Publisher odom_pub_; 16 | ros::Subscriber pose_sub_; 17 | 18 | // helper functions 19 | 20 | void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg); 21 | }; 22 | 23 | PoseToOdom::PoseToOdom() 24 | : nhp_("~") 25 | { 26 | odom_pub_ = nh_.advertise("odom", 1); 27 | pose_sub_ = nh_.subscribe("pose", 1, &PoseToOdom::poseCallback, this); 28 | } 29 | 30 | void PoseToOdom::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg) 31 | { 32 | nav_msgs::Odometry odo_msg; 33 | odo_msg.header = pose_msg->header; 34 | odo_msg.pose.pose = pose_msg->pose; 35 | 36 | odom_pub_.publish(odo_msg); 37 | } 38 | 39 | int main(int argc, char *argv[]) 40 | { 41 | ros::init(argc, argv, "pose_to_odom"); 42 | 43 | PoseToOdom pose_to_odom; 44 | 45 | ros::spin(); 46 | 47 | return 0; 48 | } 49 | 50 | -------------------------------------------------------------------------------- /src/odom_to_imu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class OdomToImu 6 | { 7 | public: 8 | 9 | OdomToImu(); 10 | 11 | private: 12 | 13 | ros::NodeHandle nh_, nhp_; 14 | 15 | ros::Subscriber odom_sub_; 16 | ros::Publisher imu_pub_; 17 | 18 | // helper functions 19 | 20 | void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg); 21 | }; 22 | 23 | OdomToImu::OdomToImu() 24 | : nhp_("~") 25 | { 26 | odom_sub_ = nh_.subscribe("odom", 100, &OdomToImu::odomCallback, this); 27 | imu_pub_ = nh_.advertise("imu", 100); 28 | } 29 | 30 | void OdomToImu::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) 31 | { 32 | sensor_msgs::Imu imu_msg; 33 | imu_msg.header = odom_msg->header; 34 | imu_msg.orientation = odom_msg->pose.pose.orientation; 35 | imu_pub_.publish( imu_msg ); 36 | } 37 | 38 | int main(int argc, char *argv[]) 39 | { 40 | ros::init(argc, argv, "odom_to_imu"); 41 | 42 | OdomToImu odom_to_imu; 43 | 44 | ROS_INFO("odom_to_imu node running..."); 45 | 46 | ros::spin(); 47 | 48 | return 0; 49 | } 50 | 51 | -------------------------------------------------------------------------------- /src/pose_to_path.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class PathBuilder 6 | { 7 | public: 8 | 9 | PathBuilder(); 10 | 11 | private: 12 | 13 | ros::NodeHandle nh_, nhp_; 14 | 15 | ros::Subscriber pose_sub_; 16 | ros::Publisher path_pub_; 17 | 18 | nav_msgs::Path path_msg_; 19 | 20 | // helper functions 21 | 22 | void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose_msg); 23 | }; 24 | 25 | PathBuilder::PathBuilder() 26 | : nhp_("~") 27 | { 28 | pose_sub_ = nh_.subscribe("pose", 100, &PathBuilder::poseCallback, this); 29 | path_pub_ = nh_.advertise("path", 100); 30 | } 31 | 32 | void PathBuilder::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose_msg) 33 | { 34 | path_msg_.header.frame_id = pose_msg->header.frame_id; 35 | 36 | geometry_msgs::PoseStamped aux; 37 | aux.header =pose_msg->header; 38 | aux.pose = pose_msg->pose.pose; 39 | path_msg_.poses.push_back( aux ); 40 | 41 | path_pub_.publish( path_msg_ ); 42 | } 43 | 44 | int main(int argc, char *argv[]) 45 | { 46 | // Override SIGINT handler 47 | ros::init(argc, argv, "path_builder"); 48 | 49 | PathBuilder path_builder; 50 | 51 | ROS_INFO("path_builder node running..."); 52 | 53 | ros::spin(); 54 | 55 | return 0; 56 | } 57 | 58 | -------------------------------------------------------------------------------- /src/odom_to_pose.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class OdomToPose 11 | { 12 | public: 13 | 14 | OdomToPose(); 15 | 16 | private: 17 | 18 | ros::NodeHandle nh_, nhp_; 19 | 20 | ros::Subscriber odom_sub_; 21 | ros::Publisher pose_pub_; 22 | 23 | tf2_ros::TransformBroadcaster broadcaster; 24 | tf2_ros::Buffer tf_buffer; 25 | tf2_ros::TransformListener listener; 26 | 27 | // helper functions 28 | 29 | void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg); 30 | 31 | std::string target_frame; 32 | }; 33 | 34 | OdomToPose::OdomToPose() 35 | : nhp_("~"), listener(tf_buffer) 36 | { 37 | odom_sub_ = nh_.subscribe("odom", 100, &OdomToPose::odomCallback, this); 38 | pose_pub_ = nh_.advertise("pose", 100); 39 | nhp_.param("target_frame", target_frame, std::string()); 40 | } 41 | 42 | void OdomToPose::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) 43 | { 44 | geometry_msgs::PoseWithCovarianceStamped pose_msg; 45 | pose_msg.header = odom_msg->header; 46 | pose_msg.pose = odom_msg->pose; 47 | 48 | if (pose_pub_.getNumSubscribers() > 0) 49 | pose_pub_.publish( pose_msg ); 50 | 51 | geometry_msgs::TransformStamped t; 52 | t.transform.rotation = odom_msg->pose.pose.orientation; 53 | t.transform.translation.x = odom_msg->pose.pose.position.x; 54 | t.transform.translation.y = odom_msg->pose.pose.position.y; 55 | t.transform.translation.z = odom_msg->pose.pose.position.z; 56 | t.header = pose_msg.header; 57 | 58 | if (!target_frame.empty()) { 59 | geometry_msgs::TransformStamped target_tf = tf_buffer.lookupTransform(odom_msg->child_frame_id, target_frame, ros::Time(0), ros::Duration(1)); 60 | tf2::Transform t1, t2; 61 | tf2::fromMsg(target_tf.transform, t2); 62 | tf2::fromMsg(t.transform, t1); 63 | t1 = t1 * t2; 64 | t.transform = tf2::toMsg(t1); 65 | t.child_frame_id = target_frame; 66 | } 67 | else 68 | { 69 | t.child_frame_id = odom_msg->child_frame_id; 70 | } 71 | 72 | broadcaster.sendTransform(t); 73 | } 74 | 75 | int main(int argc, char *argv[]) 76 | { 77 | ros::init(argc, argv, "odom_to_pose"); 78 | 79 | OdomToPose odom_to_pose; 80 | 81 | ROS_INFO("odom_to_pose node running..."); 82 | 83 | ros::spin(); 84 | 85 | return 0; 86 | } 87 | 88 | -------------------------------------------------------------------------------- /cmake-modules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /src/poseWithCovariance_to_ellipsoid.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | 8 | namespace Eigen 9 | { 10 | typedef Matrix Matrix6d; 11 | } 12 | 13 | class CovarianceEllipsoidBuilder 14 | { 15 | public: 16 | 17 | CovarianceEllipsoidBuilder(); 18 | 19 | private: 20 | 21 | ros::NodeHandle nh_, nhp_; 22 | 23 | ros::Subscriber pose_sub_; 24 | ros::Publisher covarianceEllipsoidPub_; 25 | 26 | visualization_msgs::Marker covarianceEllipsoidMsg_; 27 | 28 | // helper functions 29 | 30 | void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose_msg); 31 | }; 32 | 33 | visualization_msgs::Marker computeCovarianceEllipsoid( const geometry_msgs::PoseWithCovarianceStamped& poseWithCovarianceMsg ) { 34 | 35 | Eigen::Matrix6d covariance; 36 | for (int i=0; i < 6; ++i) { 37 | for (int j=0; j < 6; ++j) { 38 | covariance(i,j) = poseWithCovarianceMsg.pose.covariance[6*i + j]; 39 | } 40 | } 41 | 42 | geometry_msgs::Point position = poseWithCovarianceMsg.pose.pose.position; 43 | ros::Time time = poseWithCovarianceMsg.header.stamp; 44 | std::string frame_id = poseWithCovarianceMsg.header.frame_id; 45 | 46 | // get the position part of the covariance matrix 47 | Eigen::Matrix3d positionCovariance = covariance.block(0, 0, 3, 3); 48 | 49 | // Compute eigenvalues and eigenvectors (they are complex numbers) 50 | Eigen::EigenSolver eigenSolver( positionCovariance ); 51 | Eigen::VectorXcd eigValues_complex = eigenSolver.eigenvalues(); 52 | Eigen::MatrixXcd eigVectors_complex = eigenSolver.eigenvectors(); 53 | 54 | // Check eigenvalues and eigenvectors complex part is null 55 | assert( eigValues_complex.imag() == Eigen::Vector3d::Zero(3) ); 56 | assert( eigVectors_complex.imag() == Eigen::Matrix3d::Zero(3, 3) ); 57 | 58 | // keep real part of the complex eigenvalues and eigenvectors 59 | Eigen::Vector3d eigValues = eigValues_complex.real(); 60 | Eigen::Matrix3d eigVectors = eigVectors_complex.real(); 61 | 62 | Eigen::Quaterniond covarianceQuaternion ( eigVectors ); 63 | 64 | visualization_msgs::Marker covarianceEllipsoid; 65 | // Set the frame ID and timestamp. See the TF tutorials for information on these. 66 | covarianceEllipsoid.header.frame_id = frame_id; 67 | covarianceEllipsoid.header.stamp = time; 68 | 69 | // Set the namespace and id for this marker. This serves to create a unique ID 70 | // Any marker sent with the same namespace and id will overwrite the old one 71 | covarianceEllipsoid.ns = "covariance"; 72 | covarianceEllipsoid.id = 0; 73 | 74 | // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 75 | covarianceEllipsoid.type = visualization_msgs::Marker::SPHERE; 76 | 77 | // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 78 | covarianceEllipsoid.action = visualization_msgs::Marker::ADD; 79 | 80 | // painting the Gaussian Ellipsoid Marker 81 | covarianceEllipsoid.pose.position = position; 82 | covarianceEllipsoid.pose.orientation.x = covarianceQuaternion.x(); 83 | covarianceEllipsoid.pose.orientation.y = covarianceQuaternion.y(); 84 | covarianceEllipsoid.pose.orientation.z = covarianceQuaternion.z(); 85 | covarianceEllipsoid.pose.orientation.w = covarianceQuaternion.w(); 86 | double marker_scale = 100000; // scale factor to allow the ellipsoid be big enough 87 | covarianceEllipsoid.scale.x = eigValues[0] * marker_scale; 88 | covarianceEllipsoid.scale.y = eigValues[1] * marker_scale; 89 | covarianceEllipsoid.scale.z = eigValues[2] * marker_scale; 90 | 91 | // covarianceMarker.pose.orientation.x = 0; 92 | // covarianceMarker.pose.orientation.y = 0; 93 | // covarianceMarker.pose.orientation.z = 0; 94 | // covarianceMarker.pose.orientation.w = 1; 95 | // covarianceMarker.scale.x = positionCovariance(0,0) * marker_scale; 96 | // covarianceMarker.scale.y = positionCovariance(1,1) * marker_scale; 97 | // covarianceMarker.scale.z = positionCovariance(2,2) * marker_scale; 98 | covarianceEllipsoid.color.r = 1.0f; 99 | covarianceEllipsoid.color.g = 0.0f; 100 | covarianceEllipsoid.color.b = 1.0f; 101 | covarianceEllipsoid.color.a = 0.5; 102 | 103 | covarianceEllipsoid.lifetime = ros::Duration(); 104 | 105 | return covarianceEllipsoid; 106 | } 107 | 108 | CovarianceEllipsoidBuilder::CovarianceEllipsoidBuilder() 109 | : nhp_("~") 110 | { 111 | pose_sub_ = nh_.subscribe("pose", 100, &CovarianceEllipsoidBuilder::poseCallback, this); 112 | covarianceEllipsoidPub_ = nh_.advertise("covariance", 100); 113 | } 114 | 115 | void CovarianceEllipsoidBuilder::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose_msg) 116 | { 117 | covarianceEllipsoidMsg_ = computeCovarianceEllipsoid( *pose_msg ); 118 | covarianceEllipsoidPub_.publish( covarianceEllipsoidMsg_ ); 119 | } 120 | 121 | int main(int argc, char *argv[]) 122 | { 123 | // Override SIGINT handler 124 | ros::init(argc, argv, "path_builder"); 125 | 126 | CovarianceEllipsoidBuilder covarianceEllipsoidBuilder; 127 | 128 | ROS_INFO("path_builder node running..."); 129 | 130 | ros::spin(); 131 | 132 | return 0; 133 | } 134 | 135 | 136 | 137 | 138 | 139 | --------------------------------------------------------------------------------