diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..37c68a7adc5bd874357de4a2c8b2bf799f9ae669 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,215 @@ +cmake_minimum_required(VERSION 2.8.3) +project(fub_lidar_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 + geometry_msgs + rospy + sensor_msgs + std_msgs + fub_velodyne_segmenter_msgs + fub_feature_common + pole_based_localization + visualization_msgs + pcl_ros + pcl_conversions + roscpp + tf2 + tf2_ros + tf2_msgs + nodelet +) + +find_package(PCL REQUIRED COMPONENTS common search sample_consensus segmentation registration recognition) + +find_package(fub_cmake REQUIRED) +include(FUB) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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"1234 KommentareWeitersagenSpeichernAusblendenmeldencrosspost + +## * add a build_depend and a run_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 run_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 +# Pole.msg +# PoleArray.msg +# PoleMatch.msg +# PoleMatchArray.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 +# geometry_msgs# sensor_msgs# std_msgs# visualization_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 run_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( + CATKIN_DEPENDS + message_runtime + sensor_msgs + pole_based_localization +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/pole_based_localization.cpp +# ) + +## 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 +add_executable(fub_lidar_odometry + src/main.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(fub_lidar_odometry + ${catkin_LIBRARIES} + sqlite3 + pcl_registration +) + +## Declare a C++ library +add_library(fub_lidar_odometry_nodelet + src/LidarOdometry.cpp +) + +target_link_libraries(fub_lidar_odometry_nodelet + ${catkin_LIBRARIES} + sqlite3 + pcl_registration +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executables and/or libraries for installation +install( + TARGETS + fub_lidar_odometry + fub_lidar_odometry_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark other files for installation (e.g. launch and bag files, etc.) +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install( + FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pole_based_localization.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) diff --git a/include/LidarOdometry.h b/include/LidarOdometry.h new file mode 100644 index 0000000000000000000000000000000000000000..f75cf76e6d1b7f7eac29123007524e7c1bc716a7 --- /dev/null +++ b/include/LidarOdometry.h @@ -0,0 +1,35 @@ +#ifndef FUB_LIDAR_ODOMETRY_H +#define FUB_LIDAR_ODOMETRY_H + +#include <ros/ros.h> +#include <nodelet/nodelet.h> +#include <tf2_ros/transform_listener.h> + +#include <geometry_msgs/PoseWithCovarianceStamped.h> + +#include <fub_feature_common/Poles.h> +#include <fub_feature_common/Edges.h> +#include <fub_feature_common/Features.h> + + +namespace fub_lidar_odometry { + + +class LidarOdometry : public nodelet::Nodelet{ +public: + LidarOdometry(); + virtual ~LidarOdometry(); + virtual void onInit() override; + +protected: + void callbackFeatures(const fub_feature_common::Features& features); + + ros::Subscriber mFeaturesSubscriber; + std::map<long, fub_feature_common::Pole> mLastPoles; + std::map<long, fub_feature_common::Corner> mLastCorners; + std::map<long, fub_feature_common::Edge> mLastEdges; +}; +} + +#endif + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml new file mode 100644 index 0000000000000000000000000000000000000000..b2247e35f1d415b7a2d672c5f445284494924de1 --- /dev/null +++ b/nodelet_plugins.xml @@ -0,0 +1,10 @@ +<library path="lib/libfub_lidar_odometry_nodelet"> + <class name="fub_lidar_odometry/Nodelet" + type="fub_lidar_odometry::LidarOdometry" + base_class_type="nodelet::Nodelet" + > + <description> + nodelet for the lidar odometry + </description> + </class> +</library> diff --git a/package.xml b/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..ec46bcf1dd7517385c5bc76f573ce06d51f7cd7e --- /dev/null +++ b/package.xml @@ -0,0 +1,78 @@ +<?xml version="1.0"?> +<package format="2"> + <name>fub_lidar_odometry</name> + <version>0.0.0</version> + <description>Lidar odometry.</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="n.steinke@fu-berlin.de">Nicolai Steinke</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>TODO</license> + + + <!-- 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/pole_based_localization</url> --> + + + <!-- 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> --> + + + <!-- 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>geometry_msgs</build_depend> + <build_depend>rospy</build_depend> + <build_depend>sensor_msgs</build_depend> + <build_depend>std_msgs</build_depend> + <build_depend>visualization_msgs</build_depend> + <depend>fub_feature_common</depend> + <depend>pole_based_localization</depend> + <build_depend>fub_velodyne_segmenter_msgs</build_depend> + <build_export_depend>geometry_msgs</build_export_depend> + <build_export_depend>rospy</build_export_depend> + <build_export_depend>sensor_msgs</build_export_depend> + <build_export_depend>std_msgs</build_export_depend> + <build_export_depend>visualization_msgs</build_export_depend> + <exec_depend>geometry_msgs</exec_depend> + <exec_depend>rospy</exec_depend> + <exec_depend>sensor_msgs</exec_depend> + <exec_depend>std_msgs</exec_depend> + <exec_depend>visualization_msgs</exec_depend> + <exec_depend>message_runtime</exec_depend> + <depend>nodelet</depend> + + + <!-- The export tag contains other, unspecified, tags --> + <export> + <nodelet plugin="${prefix}/nodelet_plugins.xml" /> + </export> +</package> diff --git a/src/LidarOdometry.cpp b/src/LidarOdometry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7304879858f696914dc308b71783e328ccecf45a --- /dev/null +++ b/src/LidarOdometry.cpp @@ -0,0 +1,101 @@ +#include "LidarOdometry.h" +#include <pcl_ros/point_cloud.h> +#include <pcl/registration/transformation_estimation_svd.h> +#include <pcl/registration/correspondence_rejection_sample_consensus.h> +#include <pcl/registration/correspondence_rejection_median_distance.h> +#include <pcl/common/transformation_from_correspondences.h> +#include <pcl/common/poses_from_matches.h> + +/* -------------------------------------------------------------------------- */ + +/** Lidar odometry for velodyne sensors + ** + ** @ingroup @@ + */ + +namespace fub_lidar_odometry{ + +LidarOdometry::LidarOdometry() +{} + +LidarOdometry::~LidarOdometry() +{ +} + +void LidarOdometry::onInit() +{ + ros::NodeHandle nh; + mFeaturesSubscriber = nh.subscribe(nh.resolveName("/pole_based_localization/detected_features"), 11, &LidarOdometry::callbackFeatures, this); +} + + +void LidarOdometry::callbackFeatures(const fub_feature_common::Features& features) +{ +/* + // get own position + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = "base_link"; + geometry_msgs::TransformStamped transformStamped; + + // take time + auto start = std::chrono::system_clock::now(); + + try{ + // mTfBuffer.canTransform("utm", "base_link", header.stamp, ros::Duration(5.0)); + transformStamped = mTfBuffer.lookupTransform("utm", "base_link", header.stamp, ros::Duration(1.0)); + } + catch (tf2::TransformException &ex) { + ROS_WARN("%s",ex.what()); + return; + } + + geometry_msgs::PointStamped psIn, posUTM; + psIn.header = header; + tf2::doTransform(psIn, posUTM, transformStamped);*/ + + ROS_INFO("got features"); + pcl::PointCloud<pcl::PointXYZ> cloud_old; + pcl::PointCloud<pcl::PointXYZ> cloud_new; + + // correlate last features with current features + for(auto pole : features.poles.poles){ + if(mLastPoles.find(pole.trackingId) != mLastPoles.end()){ + pcl::PointXYZ po(mLastPoles.at(pole.trackingId).position.x, mLastPoles.at(pole.trackingId).position.y, 0); + pcl::PointXYZ pn(pole.position.x, pole.position.y, 0); + cloud_old.push_back(po); + cloud_new.push_back(pn); + } + } + + //TODO: edges and walls + + // calculate displacement + pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ, double> pclSVD; + pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ, double>::Matrix4 trans; + pclSVD.estimateRigidTransformation(cloud_old, cloud_new, trans); + geometry_msgs::Pose result; + + result.position.x = 0.0; + result.position.y = 0.0; + result.position.z = 0.0; + + if(!std::isnan(trans(1)) && !std::isnan(trans(12)) && !std::isnan(trans(13))){ + result.position.x = trans(12); + result.position.y = trans(13); + result.orientation.z = trans(1); + } + + result.orientation.x = 0.0; + result.orientation.y = 0.0; + result.orientation.z = 0.0; + result.orientation.w = 1.0; + + // publish odometry + ROS_ERROR_STREAM("x, y, yaw: " << result.position.x << ", " << result.position.y << ", " << result.orientation.z); +} + + +} +#include <pluginlib/class_list_macros.h> +PLUGINLIB_EXPORT_CLASS(fub_lidar_odometry::LidarOdometry, nodelet::Nodelet) diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a487bbf54db3a5497ec16ba0ffb667d17098e1a7 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,28 @@ +#include <ros/ros.h> +#include <nodelet/loader.h> + +/** Starting point for the node. It instantiates the nodelet within the node + ** (alternatively the nodelet could be run in a standalone nodelet manager). + ** + ** @param argc + ** @param argv + ** @return + ** + ** @ingroup @@ + */ +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "LidarOdometry"); + + nodelet::Loader nodelet; + nodelet::M_string remappings(ros::names::getRemappings()); + nodelet::V_string nodeletArgv(argv, argv + argc); + + std::string nodeletName = "fub_lidar_odometry/Nodelet"; + if (!nodelet.load(ros::this_node::getName(), nodeletName, remappings, nodeletArgv)) { + return -1; + } + ros::MultiThreadedSpinner spinner(4); + //spinner.spin(); // multi thread spinner + ros::spin(); // single thread spinner +}