diff --git a/planner_msgs/include/dummy b/mav_planning_rviz/COLCON_IGNORE similarity index 100% rename from planner_msgs/include/dummy rename to mav_planning_rviz/COLCON_IGNORE diff --git a/planner_msgs/CMakeLists.txt b/planner_msgs/CMakeLists.txt index 2be0de94..38d7d8b8 100644 --- a/planner_msgs/CMakeLists.txt +++ b/planner_msgs/CMakeLists.txt @@ -1,27 +1,23 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.14.4) project(planner_msgs) -find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs) +find_package(ament_cmake REQUIRED) -include_directories(include) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) -add_message_files( - DIRECTORY msg - FILES - NavigationStatus.msg - TerrainInfo.msg +rosidl_generate_interfaces(${PROJECT_NAME} + msg/NavigationStatus.msg + msg/TerrainInfo.msg + srv/SetPlannerState.srv + srv/SetService.srv + srv/SetString.srv + srv/SetVector3.srv + DEPENDENCIES std_msgs geometry_msgs ) -add_service_files( - FILES - SetString.srv - SetVector3.srv - SetService.srv - SetPlannerState.srv -) - -generate_messages(DEPENDENCIES geometry_msgs std_msgs) +ament_export_dependencies(rosidl_default_runtime) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS geometry_msgs message_runtime std_msgs) +ament_package() diff --git a/planner_msgs/package.xml b/planner_msgs/package.xml index 1f471d37..6b5dc041 100644 --- a/planner_msgs/package.xml +++ b/planner_msgs/package.xml @@ -1,5 +1,6 @@ - + + planner_msgs 0.0.1 @@ -10,18 +11,16 @@ Jaeyoung Lim BSD - catkin - message_generation - message_runtime - std_msgs + ament_cmake + + rclcpp geometry_msgs - mavros_msgs - sensor_msgs - + std_msgs + rosidl_default_generators + + rosidl_interface_packages - + ament_cmake diff --git a/terrain_navigation/CMakeLists.txt b/terrain_navigation/CMakeLists.txt index cb444653..7f196396 100644 --- a/terrain_navigation/CMakeLists.txt +++ b/terrain_navigation/CMakeLists.txt @@ -1,43 +1,104 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.14.4) project(terrain_navigation) -add_definitions(-std=c++17) -find_package(catkin REQUIRED COMPONENTS - eigen_catkin - grid_map_core - grid_map_geo -) +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -catkin_package( - INCLUDE_DIRS include - LIBRARIES terrain_navigation -) +# Set policy for 3.16 for CMP0076 defaulting to ON +cmake_policy(VERSION 3.16) -include_directories( - ${catkin_INCLUDE_DIRS} - include - ${Boost_INCLUDE_DIR} - ${Eigen_INCLUDE_DIRS} -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_geo REQUIRED) + +find_package(class_loader REQUIRED) +find_package(console_bridge REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rosbag2_storage REQUIRED) +find_package(tinyxml2 REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(action_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(GDAL 3 REQUIRED) +find_package(OpenCV REQUIRED) + +find_package(yaml_cpp_vendor REQUIRED) +link_directories(${yaml_cpp_vendor_LIBRARY_DIRS}) + +# Reverse compatability for GDAL<3.5 +# https://gdal.org/development/cmake.html +if(NOT TARGET GDAL::GDAL) + add_library(GDAL::GDAL ALIAS ${GDAL_LIBRARY}) +endif() add_library(${PROJECT_NAME} src/data_logger.cpp src/profiler.cpp src/terrain_map.cpp ) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) -if(CATKIN_ENABLE_TESTING) - # Add gtest based cpp test target and link libraries - catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp - test/test_trajectory.cpp) - # test/test_terrain_map.cpp) +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + Eigen3::Eigen + GDAL::GDAL + ${OpenCV_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC + grid_map_core + grid_map_geo +) + +# Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(GDAL) - if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${YAML_CPP_LIBRARIES} ${OpenCV_LIBRARIES}) - endif() +# Test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}-test + test/main.cpp + test/test_trajectory.cpp + # test/test_terrain_map.cpp + ) + target_link_libraries(${PROJECT_NAME}-test + ${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ) endif() + +ament_package() diff --git a/terrain_navigation/include/terrain_navigation/data_logger.h b/terrain_navigation/include/terrain_navigation/data_logger.h index 7971cb62..5a3d6664 100644 --- a/terrain_navigation/include/terrain_navigation/data_logger.h +++ b/terrain_navigation/include/terrain_navigation/data_logger.h @@ -62,7 +62,8 @@ class DataLogger { void writeToFile(const std::string path); private: - int id_{0}; + //! @todo(srmainwaring) remove unused variable + // int id_{0}; std::vector keys_; std::vector> data_list_; bool print_header_{false}; diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h index 2fe05802..cfdf9039 100644 --- a/terrain_navigation/include/terrain_navigation/path.h +++ b/terrain_navigation/include/terrain_navigation/path.h @@ -70,7 +70,7 @@ class Path { void prependSegment(const PathSegment &trajectory) { segments.insert(segments.begin(), trajectory); }; void appendSegment(const PathSegment &trajectory) { segments.push_back(trajectory); }; void appendSegment(const Path &trajectory_segments) { - for (const auto trajectory : trajectory_segments.segments) { + for (const auto& trajectory : trajectory_segments.segments) { appendSegment(trajectory); } }; @@ -178,7 +178,7 @@ class Path { */ double getLength(const int start_idx = 0) { double length{0.0}; - for (int i = start_idx; i < segments.size(); i++) { + for (size_t i = start_idx; i < segments.size(); i++) { length += segments[i].getLength(); } return length; diff --git a/terrain_navigation/include/terrain_navigation/path_segment.h b/terrain_navigation/include/terrain_navigation/path_segment.h index cc777fdb..0a2dd9a7 100644 --- a/terrain_navigation/include/terrain_navigation/path_segment.h +++ b/terrain_navigation/include/terrain_navigation/path_segment.h @@ -36,11 +36,12 @@ #ifndef PATH_SEGMENT_H #define PATH_SEGMENT_H -#include #include #include #include +#include + struct State { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d position; diff --git a/terrain_navigation/include/terrain_navigation/primitive.h b/terrain_navigation/include/terrain_navigation/primitive.h index 41fa2318..da3385ec 100644 --- a/terrain_navigation/include/terrain_navigation/primitive.h +++ b/terrain_navigation/include/terrain_navigation/primitive.h @@ -72,7 +72,8 @@ class Primitive { std::vector getMotionPrimitives() { std::vector all_primitives; if (has_child()) { - int i = 0; + //! @todo(srmainwaring) remove set but unused variable + // int i = 0; for (const auto &child : child_primitives) { std::vector extended_primitives = child->getMotionPrimitives(); // Append current segment @@ -81,7 +82,8 @@ class Primitive { primitive.validity = validity && primitive.validity; all_primitives.push_back(primitive); } - i++; + //! @todo(srmainwaring) remove set but unused variable + // i++; } } else { // Append primitive segments Path trajectory_segments; @@ -102,6 +104,7 @@ class Primitive { for (auto &child : child_primitives) { if (child->valid()) return child; } + return nullptr; } std::shared_ptr copy() const { diff --git a/terrain_navigation/include/terrain_navigation/terrain_map.h b/terrain_navigation/include/terrain_navigation/terrain_map.h index 7354fe8e..da134245 100644 --- a/terrain_navigation/include/terrain_navigation/terrain_map.h +++ b/terrain_navigation/include/terrain_navigation/terrain_map.h @@ -36,17 +36,26 @@ #ifndef TERRAIN_MAP_H #define TERRAIN_MAP_H -#include +#include +#include + +#include #include #include +#if __APPLE__ +#include +#include +#include +#include +#include +#else #include #include #include #include #include -#include -#include +#endif class TerrainMap : public GridMapGeo { public: diff --git a/terrain_navigation/include/terrain_navigation/viewpoint.h b/terrain_navigation/include/terrain_navigation/viewpoint.h index ae6fac67..e623d668 100644 --- a/terrain_navigation/include/terrain_navigation/viewpoint.h +++ b/terrain_navigation/include/terrain_navigation/viewpoint.h @@ -41,14 +41,16 @@ #ifndef TERRAIN_PLANNER_VIEWPOINT_H #define TERRAIN_PLANNER_VIEWPOINT_H -#include #include +#include + +#include + #include +#include #include #include #include -#include -#include "opencv2/core.hpp" class ViewPoint { public: diff --git a/terrain_navigation/include/terrain_navigation/visualization.h b/terrain_navigation/include/terrain_navigation/visualization.h index 90c4ea24..3c3f52e7 100644 --- a/terrain_navigation/include/terrain_navigation/visualization.h +++ b/terrain_navigation/include/terrain_navigation/visualization.h @@ -42,30 +42,31 @@ #ifndef VISUALIZATION_H #define VISUALIZATION_H -#include "geometry_msgs/Point.h" -#include "terrain_navigation/viewpoint.h" -#include "visualization_msgs/Marker.h" +#include -geometry_msgs::Point toPoint(const Eigen::Vector3d &p) { - geometry_msgs::Point position; +#include +#include + +geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { + geometry_msgs::msg::Point position; position.x = p(0); position.y = p(1); position.z = p(2); return position; } -visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, +visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { double scale{15}; // Size of the viewpoint markers - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); + marker.header.stamp = rclcpp::Clock().now(); marker.ns = "my_namespace"; marker.id = id; - marker.type = visualization_msgs::Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; const Eigen::Vector3d position = viewpoint.getCenterLocal(); - std::vector points; + std::vector points; std::vector corner_ray_vectors = viewpoint.getCornerRayVectors(); std::vector vertex; for (auto &corner_ray : corner_ray_vectors) { diff --git a/terrain_navigation/package.xml b/terrain_navigation/package.xml index 9bf4d6a3..c5c6cf75 100644 --- a/terrain_navigation/package.xml +++ b/terrain_navigation/package.xml @@ -1,25 +1,39 @@ - + + + terrain_navigation 0.0.1 Steep terrain navigation package - Jaeyoung Lim - Jaeyoung Lim - BSD-3 https://github.com/Jaeyoung-Lim/steep-terrain-navigation https://github.com/Jaeyoung-Lim/steep-terrain-navigation/issues - - catkin - eigen_catkin + ament_cmake + grid_map_core grid_map_geo libgdal-dev - eigen_catkin - gdal-bin + + + class_loader + console_bridge + eigen + libgdal-dev + pluginlib + rclcpp + rosbag2_storage + tinyxml2 + visualization_msgs + + action_msgs + grid_map_msgs + nav2_msgs + sensor_msgs + + ament_cmake diff --git a/terrain_navigation/src/data_logger.cpp b/terrain_navigation/src/data_logger.cpp index 5b55cf55..777c8571 100644 --- a/terrain_navigation/src/data_logger.cpp +++ b/terrain_navigation/src/data_logger.cpp @@ -38,6 +38,7 @@ * @author Jaeyoung Lim */ #include "terrain_navigation/data_logger.h" + #include #include diff --git a/terrain_navigation/src/terrain_map.cpp b/terrain_navigation/src/terrain_map.cpp index a3c17fed..fc68e45b 100644 --- a/terrain_navigation/src/terrain_map.cpp +++ b/terrain_navigation/src/terrain_map.cpp @@ -39,6 +39,7 @@ */ #include "terrain_navigation/terrain_map.h" + #include TerrainMap::TerrainMap() : GridMapGeo() {} @@ -110,8 +111,8 @@ void TerrainMap::AddLayerNormals(const std::string reference_layer) { grid_map::Matrix &layer_normal_y = grid_map_[reference_layer + "_normal_y"]; grid_map::Matrix &layer_normal_z = grid_map_[reference_layer + "_normal_z"]; - unsigned width = grid_map_.getSize()(0); - unsigned height = grid_map_.getSize()(1); + int width = grid_map_.getSize()(0); + int height = grid_map_.getSize()(1); double resolution = grid_map_.getResolution(); // Compute normals from elevation map // Surface normal calculation from: https://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml diff --git a/terrain_navigation/test/test_trajectory.cpp b/terrain_navigation/test/test_trajectory.cpp index 1d298d8c..4848dc70 100644 --- a/terrain_navigation/test/test_trajectory.cpp +++ b/terrain_navigation/test/test_trajectory.cpp @@ -3,6 +3,16 @@ #include #include +//! @todo(srmainwaring)check includes +// #include "terrain_navigation/data_logger.h" +// #include "terrain_navigation/path_segment.h" +// #include "terrain_navigation/path.h" +// #include "terrain_navigation/primitive.h" +// #include "terrain_navigation/profiler.h" +// #include "terrain_navigation/terrain_map.h" +// #include "terrain_navigation/viewpoint.h" +// #include "terrain_navigation/visualization.h" + #define FLOAT_EPS 1e-6 TEST(PathSegmentTest, getArcCenter) { diff --git a/terrain_navigation_ros/CMakeLists.txt b/terrain_navigation_ros/CMakeLists.txt index ffacf919..a7eeda58 100644 --- a/terrain_navigation_ros/CMakeLists.txt +++ b/terrain_navigation_ros/CMakeLists.txt @@ -1,56 +1,135 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.14.4) project(terrain_navigation_ros) -add_definitions(-std=c++17) + +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Set policy for 3.16 for CMP0076 defaulting to ON +cmake_policy(VERSION 3.16) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake") +# list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS +# path/to/file.cmake" +# other/pathto/file.cmake" +# ) +find_package(ament_cmake REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_geo REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_cv REQUIRED) +# find_package(grid_map_pcl REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(interactive_markers REQUIRED) find_package(ompl REQUIRED) -find_package(catkin REQUIRED dynamic_reconfigure) -generate_dynamic_reconfigure_options( - cfg/HeightRateTuning.cfg -) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(terrain_navigation REQUIRED) +find_package(terrain_planner REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) + +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(planner_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + find_package(Boost REQUIRED COMPONENTS serialization system filesystem) +find_package(Eigen3 REQUIRED) find_package(GeographicLib REQUIRED) -include(CheckGeographicLibDatasets) +# include(CheckGeographicLibDatasets) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(ODE REQUIRED ode) + +add_library(${PROJECT_NAME} + src/terrain_planner.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + ${Boost_LIBRARIES} + ${GeographicLib_LIBRARIES} + ${OMPL_LIBRARIES} + ${OpenCV_LIBRARIES} + Eigen3::Eigen + tf2_eigen::tf2_eigen + ${geographic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${grid_map_msgs_TARGETS} + ${mavros_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${planner_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} +) -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - tf +ament_target_dependencies(${PROJECT_NAME} PUBLIC grid_map_core - grid_map_cv - grid_map_msgs - planner_msgs grid_map_ros - grid_map_pcl - grid_map_geo - interactive_markers terrain_navigation terrain_planner - dynamic_reconfigure ) -catkin_package( - LIBRARIES terrain_navigation - CATKIN_DEPENDS roscpp rospy dynamic_reconfigure +add_executable(terrain_planner_node + src/terrain_planner_node.cpp ) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIR} - ${Eigen_INCLUDE_DIRS} - ${GeographicLib_INCLUDE_DIRS} - ${OMPL_INCLUDE_DIR} +target_link_libraries(terrain_planner_node PUBLIC + ${PROJECT_NAME} + ${ODE_LIBRARIES} ) -add_library(${PROJECT_NAME} - src/terrain_planner.cpp +target_link_directories(terrain_planner_node PUBLIC + ${ODE_LIBRARY_DIRS} + ${yaml_cpp_vendor_LIBRARY_DIRS} ) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${GeographicLib_LIBRARIES} ${OMPL_LIBRARIES} ${Boost_LIBRARIES}) -add_executable(terrain_planner_node - src/terrain_planner_node.cpp +# Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) -add_dependencies(terrain_planner_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY}) -target_link_libraries(terrain_planner_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES}) + +install( + TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +install( + TARGETS + terrain_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/terrain_navigation_ros/cmake/FindGeographicLib.cmake b/terrain_navigation_ros/cmake/_FindGeographicLib.cmake similarity index 100% rename from terrain_navigation_ros/cmake/FindGeographicLib.cmake rename to terrain_navigation_ros/cmake/_FindGeographicLib.cmake diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h index 171346f3..4eb0533e 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h @@ -42,99 +42,138 @@ #ifndef TERRAIN_PLANNER_H #define TERRAIN_PLANNER_H -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include "terrain_planner/common.h" -#include "terrain_planner/terrain_ompl_rrt.h" -#include "terrain_planner/visualization.h" - -#include -#include -#include -#include +#include #include #include -#include -// #include "terrain_navigation_ros/HeightRateTuningConfig.h" +#include +#include +#include +#include -enum class PLANNER_MODE { ACTIVE_MAPPING, EMERGENCY_ABORT, EXHAUSTIVE, GLOBAL, GLOBAL_REPLANNING, RANDOM, RETURN }; +#include -enum class PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 }; +#include +#include + +#include +#include +#include + +enum class PLANNER_MODE { + ACTIVE_MAPPING, + EMERGENCY_ABORT, + EXHAUSTIVE, + GLOBAL, + GLOBAL_REPLANNING, + RANDOM, + RETURN +}; + +enum class PLANNER_STATE { + HOLD = 1, + NAVIGATE = 2, + ROLLOUT = 3, + ABORT = 4, + RETURN = 5 +}; -class TerrainPlanner { +class TerrainPlanner : public rclcpp::Node { public: - TerrainPlanner(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); - virtual ~TerrainPlanner(); - void Init(); + TerrainPlanner(); + + void init(); private: - void cmdloopCallback(const ros::TimerEvent &event); - void statusloopCallback(const ros::TimerEvent &event); - void plannerloopCallback(const ros::TimerEvent &event); + // void cmdloopCallback(const ros::TimerEvent &event); + // void statusloopCallback(const ros::TimerEvent &event); + // void plannerloopCallback(const ros::TimerEvent &event); + void cmdloopCallback(); + void statusloopCallback(); + void plannerloopCallback(); + void publishTrajectory(std::vector trajectory); + // States from vehicle - void mavLocalPoseCallback(const geometry_msgs::PoseStamped &msg); - void mavGlobalPoseCallback(const sensor_msgs::NavSatFix &msg); - void mavtwistCallback(const geometry_msgs::TwistStamped &msg); - void mavstateCallback(const mavros_msgs::State::ConstPtr &msg); - void mavGlobalOriginCallback(const geographic_msgs::GeoPointStampedConstPtr &msg); - void mavMissionCallback(const mavros_msgs::WaypointListPtr &msg); - void mavImageCapturedCallback(const mavros_msgs::CameraImageCaptured::ConstPtr &msg); - bool setLocationCallback(planner_msgs::SetString::Request &req, planner_msgs::SetString::Response &res); - bool setMaxAltitudeCallback(planner_msgs::SetString::Request &req, planner_msgs::SetString::Response &res); - bool setGoalCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res); - bool setStartCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res); - bool setCurrentSegmentCallback(planner_msgs::SetService::Request &req, planner_msgs::SetService::Response &res); - bool setStartLoiterCallback(planner_msgs::SetService::Request &req, planner_msgs::SetService::Response &res); - bool setPlanningCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res); - bool setPlannerStateCallback(planner_msgs::SetPlannerState::Request &req, - planner_msgs::SetPlannerState::Response &res); - bool setPathCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res); - - void MapPublishOnce(const ros::Publisher &pub, const grid_map::GridMap &map); - void publishPositionHistory(ros::Publisher &pub, const Eigen::Vector3d &position, - std::vector &history_vector); - void publishPositionSetpoints(const ros::Publisher &pub, const Eigen::Vector3d &position, + void mavLocalPoseCallback(const geometry_msgs::msg::PoseStamped &msg); + void mavGlobalPoseCallback(const sensor_msgs::msg::NavSatFix &msg); + void mavtwistCallback(const geometry_msgs::msg::TwistStamped &msg); + void mavstateCallback(const mavros_msgs::msg::State &msg); + void mavGlobalOriginCallback(const geographic_msgs::msg::GeoPointStamped &msg); + void mavMissionCallback(const mavros_msgs::msg::WaypointList &msg); + void mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured &msg); + + bool setLocationCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setMaxAltitudeCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setGoalCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setStartCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setCurrentSegmentCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setStartLoiterCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setPlanningCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setPlannerStateCallback( + const std::shared_ptr req, + std::shared_ptr res); + bool setPathCallback( + const std::shared_ptr req, + std::shared_ptr res); + + void MapPublishOnce(rclcpp::Publisher::SharedPtr pub, const grid_map::GridMap &map); + void publishPositionHistory(rclcpp::Publisher::SharedPtr, const Eigen::Vector3d &position, + std::vector &history_vector); + void publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, const double curvature); - void publishGlobalPositionSetpoints(const ros::Publisher &pub, const double latitude, const double longitude, + void publishGlobalPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const double latitude, const double longitude, const double altitude, const Eigen::Vector3d &velocity, const double curvature); - void publishReferenceMarker(const ros::Publisher &pub, const Eigen::Vector3d &position, + void publishReferenceMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, const double curvature); - void publishVelocityMarker(const ros::Publisher &pub, const Eigen::Vector3d &position, + void publishVelocityMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const Eigen::Vector3d &velocity); void publishPathSetpoints(const Eigen::Vector3d &position, const Eigen::Vector3d &velocity); - void publishPathSegments(ros::Publisher &pub, Path &trajectory); - void publishGoal(const ros::Publisher &pub, const Eigen::Vector3d &position, const double radius, + void publishPathSegments(rclcpp::Publisher::SharedPtr, Path &trajectory); + void publishGoal(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const double radius, Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "goal"); - void publishRallyPoints(const ros::Publisher &pub, const std::vector &positions, const double radius, + void publishRallyPoints(rclcpp::Publisher::SharedPtr pub, const std::vector &positions, const double radius, Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "rallypoints"); - visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d &position, const double radius, + visualization_msgs::msg::Marker getGoalMarker(const int id, const Eigen::Vector3d &position, const double radius, const Eigen::Vector3d color); void generateCircle(const Eigen::Vector3d end_position, const Eigen::Vector3d end_velocity, const Eigen::Vector3d center_pos, PathSegment &trajectory); PathSegment generateArcTrajectory(Eigen::Vector3d rates, const double horizon, Eigen::Vector3d current_pos, Eigen::Vector3d current_vel, const double dt = 0.1); - // void dynamicReconfigureCallback(terrain_navigation_ros::HeightRateTuningConfig &config, uint32_t level); void printPlannerState(PLANNER_STATE state) { switch (state) { @@ -150,6 +189,9 @@ class TerrainPlanner { case PLANNER_STATE::ABORT: std::cout << "PLANNER_STATE::ABORT" << std::endl; break; + case PLANNER_STATE::RETURN: + std::cout << "PLANNER_STATE::RETURN" << std::endl; + break; } } @@ -157,64 +199,68 @@ class TerrainPlanner { PLANNER_STATE finiteStateMachine(const PLANNER_STATE current_state, const PLANNER_STATE query_state); - ros::NodeHandle nh_; - ros::NodeHandle nh_private_; - ros::Publisher vehicle_path_pub_; - ros::Publisher grid_map_pub_; - ros::Publisher vehicle_pose_pub_; - ros::Publisher camera_pose_pub_; - ros::Publisher posehistory_pub_; - ros::Publisher referencehistory_pub_; - ros::Publisher position_setpoint_pub_; - ros::Publisher global_position_setpoint_pub_; - ros::Publisher position_target_pub_; - ros::Publisher path_target_pub_; - ros::Publisher planner_status_pub_; - ros::Publisher goal_pub_; - ros::Publisher rallypoint_pub_; - ros::Publisher candidate_goal_pub_; - ros::Publisher candidate_start_pub_; - ros::Publisher viewpoint_pub_; - ros::Publisher planned_viewpoint_pub_; - ros::Publisher tree_pub_; - ros::Publisher vehicle_velocity_pub_; - ros::Publisher path_segment_pub_; - ros::Subscriber mavlocalpose_sub_; - ros::Subscriber mavglobalpose_sub_; - ros::Subscriber mavtwist_sub_; - ros::Subscriber mavstate_sub_; - ros::Subscriber mavmission_sub_; - ros::Subscriber global_origin_sub_; - ros::Subscriber image_captured_sub_; - - ros::ServiceServer setlocation_serviceserver_; - ros::ServiceServer setmaxaltitude_serviceserver_; - ros::ServiceServer setgoal_serviceserver_; - ros::ServiceServer setstart_serviceserver_; - ros::ServiceServer setstartloiter_serviceserver_; - ros::ServiceServer setplanning_serviceserver_; - ros::ServiceServer updatepath_serviceserver_; - ros::ServiceServer setcurrentsegment_serviceserver_; - ros::ServiceServer setplannerstate_service_server_; - ros::ServiceClient msginterval_serviceclient_; - - ros::Timer cmdloop_timer_, statusloop_timer_, plannerloop_timer_; - ros::Time plan_time_; - ros::Time last_triggered_time_; + rclcpp::Publisher::SharedPtr vehicle_path_pub_; + rclcpp::Publisher::SharedPtr grid_map_pub_; + rclcpp::Publisher::SharedPtr vehicle_pose_pub_; + rclcpp::Publisher::SharedPtr camera_pose_pub_; + rclcpp::Publisher::SharedPtr posehistory_pub_; + rclcpp::Publisher::SharedPtr referencehistory_pub_; + rclcpp::Publisher::SharedPtr position_setpoint_pub_; + rclcpp::Publisher::SharedPtr global_position_setpoint_pub_; + rclcpp::Publisher::SharedPtr position_target_pub_; + rclcpp::Publisher::SharedPtr path_target_pub_; + rclcpp::Publisher::SharedPtr planner_status_pub_; + rclcpp::Publisher::SharedPtr goal_pub_; + rclcpp::Publisher::SharedPtr rallypoint_pub_; + rclcpp::Publisher::SharedPtr candidate_goal_pub_; + rclcpp::Publisher::SharedPtr candidate_start_pub_; + rclcpp::Publisher::SharedPtr viewpoint_pub_; + rclcpp::Publisher::SharedPtr planned_viewpoint_pub_; + rclcpp::Publisher::SharedPtr tree_pub_; + rclcpp::Publisher::SharedPtr vehicle_velocity_pub_; + rclcpp::Publisher::SharedPtr path_segment_pub_; + + rclcpp::Subscription::SharedPtr mavlocalpose_sub_; + rclcpp::Subscription::SharedPtr mavglobalpose_sub_; + rclcpp::Subscription::SharedPtr mavtwist_sub_; + rclcpp::Subscription::SharedPtr mavstate_sub_; + rclcpp::Subscription::SharedPtr mavmission_sub_; + rclcpp::Subscription::SharedPtr global_origin_sub_; + rclcpp::Subscription::SharedPtr image_captured_sub_; + + rclcpp::Service::SharedPtr setlocation_serviceserver_; + rclcpp::Service::SharedPtr setmaxaltitude_serviceserver_; + rclcpp::Service::SharedPtr setgoal_serviceserver_; + rclcpp::Service::SharedPtr setstart_serviceserver_; + rclcpp::Service::SharedPtr setstartloiter_serviceserver_; + rclcpp::Service::SharedPtr setplanning_serviceserver_; + rclcpp::Service::SharedPtr updatepath_serviceserver_; + rclcpp::Service::SharedPtr setcurrentsegment_serviceserver_; + rclcpp::Service::SharedPtr setplannerstate_service_server_; + + rclcpp::Client::SharedPtr msginterval_serviceclient_; + + // ros::Timer cmdloop_timer_, statusloop_timer_, plannerloop_timer_; + rclcpp::TimerBase::SharedPtr cmdloop_timer_; + rclcpp::TimerBase::SharedPtr statusloop_timer_; + rclcpp::TimerBase::SharedPtr plannerloop_timer_; + + rclcpp::Time plan_time_; + rclcpp::Time last_triggered_time_; Eigen::Vector3d goal_pos_{Eigen::Vector3d(0.0, 0.0, 20.0)}; Eigen::Vector3d start_pos_{Eigen::Vector3d(0.0, 0.0, 20.0)}; Eigen::Vector3d home_position_{Eigen::Vector3d(0.0, 0.0, 20.0)}; double home_position_radius_{0.0}; Eigen::Vector3d tracking_error_{Eigen::Vector3d::Zero()}; - ros::CallbackQueue plannerloop_queue_; - ros::CallbackQueue statusloop_queue_; - ros::CallbackQueue cmdloop_queue_; - std::unique_ptr plannerloop_spinner_; - std::unique_ptr statusloop_spinner_; - std::unique_ptr cmdloop_spinner_; - - // dynamic_reconfigure::Server server; - // dynamic_reconfigure::Server::CallbackType f; + // ros::CallbackQueue plannerloop_queue_; + // ros::CallbackQueue statusloop_queue_; + // ros::CallbackQueue cmdloop_queue_; + // std::unique_ptr plannerloop_spinner_; + // std::unique_ptr statusloop_spinner_; + // std::unique_ptr cmdloop_spinner_; + rclcpp::executors::SingleThreadedExecutor cmdloop_executor_; + rclcpp::executors::SingleThreadedExecutor statusloop_executor_; + rclcpp::executors::SingleThreadedExecutor plannerloop_executor_; PLANNER_MODE planner_mode_{PLANNER_MODE::EXHAUSTIVE}; PLANNER_STATE planner_state_{PLANNER_STATE::HOLD}; @@ -229,7 +275,7 @@ class TerrainPlanner { Path reference_primitive_; Path candidate_primitive_; Path rollout_primitive_; - mavros_msgs::State current_state_; + mavros_msgs::msg::State current_state_; std::optional enu_; std::mutex goal_mutex_; // protects g_i @@ -237,8 +283,8 @@ class TerrainPlanner { std::vector vehicle_position_history_; std::vector added_viewpoint_list; std::vector planned_viewpoint_list; - std::vector posehistory_vector_; - std::vector referencehistory_vector_; + std::vector posehistory_vector_; + std::vector referencehistory_vector_; std::vector viewpoints_; Eigen::Vector3d vehicle_position_{Eigen::Vector3d::Zero()}; Eigen::Vector3d vehicle_velocity_{Eigen::Vector3d::Zero()}; diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h index 73cc22ed..9f27a2a2 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h @@ -34,30 +34,36 @@ #ifndef TERRAIN_NAVIGATION_ROS_VISUALIZATION_H #define TERRAIN_NAVIGATION_ROS_VISUALIZATION_H -#include #include -#include -#include -#include +#include +#include +#include -inline geometry_msgs::Point toPoint(const Eigen::Vector3d &p) { - geometry_msgs::Point position; +#include +#include + + +inline geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { + geometry_msgs::msg::Point position; position.x = p(0); position.y = p(1); position.z = p(2); return position; } -inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d &position, - const Eigen::Vector4d &attitude, std::string mesh_resource_path) { +inline void publishVehiclePose( + rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, + const Eigen::Vector4d &attitude, + std::string mesh_resource_path) { Eigen::Vector4d mesh_attitude = quatMultiplication(attitude, Eigen::Vector4d(std::cos(M_PI / 2), 0.0, 0.0, std::sin(M_PI / 2))); - geometry_msgs::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude); - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); + geometry_msgs::msg::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude); + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Clock().now(); marker.header.frame_id = "map"; - marker.type = visualization_msgs::Marker::MESH_RESOURCE; + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; marker.ns = "my_namespace"; marker.mesh_resource = "package://terrain_planner/" + mesh_resource_path; marker.scale.x = 10.0; @@ -68,21 +74,22 @@ inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d & marker.color.g = 0.5; marker.color.b = 0.5; marker.pose = vehicle_pose; - pub.publish(marker); + pub->publish(marker); } -inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { +inline visualization_msgs::msg::Marker Viewpoint2MarkerMsg( + int id, ViewPoint &viewpoint, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { double scale{15}; // Size of the viewpoint markers - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); + marker.header.stamp = rclcpp::Clock().now(); marker.ns = "my_namespace"; marker.id = id; - marker.type = visualization_msgs::Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; const Eigen::Vector3d position = viewpoint.getCenterLocal(); - std::vector points; + std::vector points; std::vector corner_ray_vectors = viewpoint.getCornerRayVectors(); std::vector vertex; for (auto &corner_ray : corner_ray_vectors) { @@ -111,33 +118,37 @@ inline visualization_msgs::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoi return marker; } -inline void publishCameraView(const ros::Publisher pub, const Eigen::Vector3d &position, - const Eigen::Vector4d &attitude) { - visualization_msgs::Marker marker; +inline void publishCameraView( + rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, + const Eigen::Vector4d &attitude) { + visualization_msgs::msg::Marker marker; ViewPoint viewpoint(-1, position, attitude); marker = Viewpoint2MarkerMsg(viewpoint.getIndex(), viewpoint); - pub.publish(marker); + pub->publish(marker); } -inline void publishViewpoints(const ros::Publisher pub, std::vector &viewpoint_vector, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { - visualization_msgs::MarkerArray msg; +inline void publishViewpoints( + rclcpp::Publisher::SharedPtr pub, + std::vector &viewpoint_vector, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { + visualization_msgs::msg::MarkerArray msg; - std::vector marker; - visualization_msgs::Marker mark; - mark.action = visualization_msgs::Marker::DELETEALL; + std::vector marker; + visualization_msgs::msg::Marker mark; + mark.action = visualization_msgs::msg::Marker::DELETEALL; marker.push_back(mark); msg.markers = marker; - pub.publish(msg); + pub->publish(msg); - std::vector viewpoint_marker_vector; + std::vector viewpoint_marker_vector; int i = 0; for (auto viewpoint : viewpoint_vector) { viewpoint_marker_vector.insert(viewpoint_marker_vector.begin(), Viewpoint2MarkerMsg(i, viewpoint, color)); i++; } msg.markers = viewpoint_marker_vector; - pub.publish(msg); + pub->publish(msg); } #endif diff --git a/terrain_navigation_ros/launch/test_terrain_planner.launch b/terrain_navigation_ros/launch/test_terrain_planner.launch index 84c0a99d..c223e899 100644 --- a/terrain_navigation_ros/launch/test_terrain_planner.launch +++ b/terrain_navigation_ros/launch/test_terrain_planner.launch @@ -9,36 +9,37 @@ - + - - - + + + - - - - - - - + + + + + + + - - - - + + + + - - - - + + + + - - + + + diff --git a/terrain_navigation_ros/package.xml b/terrain_navigation_ros/package.xml index 7358ced5..65a04574 100644 --- a/terrain_navigation_ros/package.xml +++ b/terrain_navigation_ros/package.xml @@ -1,50 +1,57 @@ - + + + terrain_navigation_ros 0.0.1 Steep terrain navigation package - Jaeyoung Lim - Jaeyoung Lim - BSD-3 - https://github.com/Jaeyoung-Lim/steep-terrain-navigation https://github.com/Jaeyoung-Lim/steep-terrain-navigation/issues - - catkin - roscpp - rospy - std_msgs - sensor_msgs - mavros - mavros_extras + ament_cmake + + geographic_msgs + geometry_msgs + grid_map_msgs mavros_msgs nav_msgs - geometry_msgs + planner_msgs + sensor_msgs + tf2_geometry_msgs + visualization_msgs + + eigen grid_map_core grid_map_cv - grid_map_ros + grid_map_geo grid_map_msgs grid_map_pcl - grid_map_geo - tf + grid_map_ros + mavros + mavros_extras + ompl planner_msgs + rclcpp + rclpy terrain_navigation terrain_planner - dynamic_reconfigure - ompl - roscpp - rospy - grid_map_visualization - grid_map_rviz_plugin - planner_msgs - terrain_navigation - terrain_planner - dynamic_reconfigure - mav_planning_rviz - ompl + tf2_eigen + yaml_cpp_vendor + + dynamic_reconfigure + grid_map_visualization + grid_map_rviz_plugin + mav_planning_rviz + planner_msgs + ompl + rclcpp + rclpy + terrain_navigation + terrain_planner + + ament_cmake diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp index db17cef6..0bea9a1a 100644 --- a/terrain_navigation_ros/src/terrain_planner.cpp +++ b/terrain_navigation_ros/src/terrain_planner.cpp @@ -40,86 +40,123 @@ */ #include "terrain_navigation_ros/terrain_planner.h" + +#include +#include +#include + #include "terrain_navigation_ros/geo_conversions.h" #include "terrain_navigation_ros/visualization.h" -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include -#include -#include +#include #include -TerrainPlanner::TerrainPlanner(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) - : nh_(nh), nh_private_(nh_private) { - vehicle_path_pub_ = nh_.advertise("vehicle_path", 1); - - grid_map_pub_ = nh_.advertise("grid_map", 1, true); - - posehistory_pub_ = nh_.advertise("geometric_controller/path", 10); - referencehistory_pub_ = nh_.advertise("reference/path", 10); - position_target_pub_ = nh_.advertise("position_target", 1); - vehicle_velocity_pub_ = nh_.advertise("vehicle_velocity", 1); - goal_pub_ = nh_.advertise("goal_marker", 1); - rallypoint_pub_ = nh_.advertise("rallypoints_marker", 1); - candidate_goal_pub_ = nh_.advertise("candidate_goal_marker", 1); - candidate_start_pub_ = nh_.advertise("candidate_start_marker", 1); - mavstate_sub_ = - nh_.subscribe("mavros/state", 1, &TerrainPlanner::mavstateCallback, this, ros::TransportHints().tcpNoDelay()); - mavmission_sub_ = nh_.subscribe("mavros/mission/waypoints", 1, &TerrainPlanner::mavMissionCallback, this, - ros::TransportHints().tcpNoDelay()); - position_setpoint_pub_ = nh_.advertise("mavros/setpoint_raw/local", 1); - global_position_setpoint_pub_ = nh_.advertise("mavros/setpoint_raw/global", 1); - path_target_pub_ = nh_.advertise("mavros/trajectory/generated", 1); - vehicle_pose_pub_ = nh_.advertise("vehicle_pose_marker", 1); - camera_pose_pub_ = nh_.advertise("camera_pose_marker", 1); - planner_status_pub_ = nh_.advertise("planner_status", 1); - viewpoint_pub_ = nh_.advertise("viewpoints", 1); - planned_viewpoint_pub_ = nh_.advertise("planned_viewpoints", 1); - path_segment_pub_ = nh_.advertise("path_segments", 1); - tree_pub_ = nh_.advertise("tree", 1); - - mavlocalpose_sub_ = nh_.subscribe("mavros/local_position/pose", 1, &TerrainPlanner::mavLocalPoseCallback, this, - ros::TransportHints().tcpNoDelay()); - mavglobalpose_sub_ = nh_.subscribe("mavros/global_position/global", 1, &TerrainPlanner::mavGlobalPoseCallback, this, - ros::TransportHints().tcpNoDelay()); - mavtwist_sub_ = nh_.subscribe("mavros/local_position/velocity_local", 1, &TerrainPlanner::mavtwistCallback, this, - ros::TransportHints().tcpNoDelay()); - global_origin_sub_ = nh_.subscribe("mavros/global_position/gp_origin", 1, &TerrainPlanner::mavGlobalOriginCallback, - this, ros::TransportHints().tcpNoDelay()); - image_captured_sub_ = nh_.subscribe("mavros/camera/image_captured", 1, &TerrainPlanner::mavImageCapturedCallback, - this, ros::TransportHints().tcpNoDelay()); - - setlocation_serviceserver_ = - nh_.advertiseService("/terrain_planner/set_location", &TerrainPlanner::setLocationCallback, this); - setmaxaltitude_serviceserver_ = - nh_.advertiseService("/terrain_planner/set_max_altitude", &TerrainPlanner::setMaxAltitudeCallback, this); - setgoal_serviceserver_ = nh_.advertiseService("/terrain_planner/set_goal", &TerrainPlanner::setGoalCallback, this); - setstart_serviceserver_ = nh_.advertiseService("/terrain_planner/set_start", &TerrainPlanner::setStartCallback, this); - setcurrentsegment_serviceserver_ = - nh_.advertiseService("/terrain_planner/set_current_segment", &TerrainPlanner::setCurrentSegmentCallback, this); - setstartloiter_serviceserver_ = - nh_.advertiseService("/terrain_planner/set_start_loiter", &TerrainPlanner::setStartLoiterCallback, this); - setplannerstate_service_server_ = - nh_.advertiseService("/terrain_planner/set_planner_state", &TerrainPlanner::setPlannerStateCallback, this); - setplanning_serviceserver_ = - nh_.advertiseService("/terrain_planner/trigger_planning", &TerrainPlanner::setPlanningCallback, this); - updatepath_serviceserver_ = nh_.advertiseService("/terrain_planner/set_path", &TerrainPlanner::setPathCallback, this); - msginterval_serviceclient_ = nh_.serviceClient("mavros/cmd/command"); +using std::placeholders::_1; +using std::placeholders::_2; +using namespace std::chrono_literals; + + +TerrainPlanner::TerrainPlanner() + : Node("terrain_planner") { + + vehicle_path_pub_ = this->create_publisher("vehicle_path", 1); + + //! @todo(srmainwaring) add latching + grid_map_pub_ = this->create_publisher("grid_map", 1); + + posehistory_pub_ = this->create_publisher("geometric_controller/path", 10); + referencehistory_pub_ = this->create_publisher("reference/path", 10); + position_target_pub_ = this->create_publisher("position_target", 1); + vehicle_velocity_pub_ = this->create_publisher("vehicle_velocity", 1); + goal_pub_ = this->create_publisher("goal_marker", 1); + rallypoint_pub_ = this->create_publisher("rallypoints_marker", 1); + candidate_goal_pub_ = this->create_publisher("candidate_goal_marker", 1); + candidate_start_pub_ = this->create_publisher("candidate_start_marker", 1); + + mavstate_sub_ = this->create_subscription( + "mavros/state", 1, + std::bind(&TerrainPlanner::mavstateCallback, this, _1)); + mavmission_sub_ = this->create_subscription( + "mavros/mission/waypoints", 1, + std::bind(&TerrainPlanner::mavMissionCallback, this, _1)); + + position_setpoint_pub_ = this->create_publisher("mavros/setpoint_raw/local", 1); + global_position_setpoint_pub_ = this->create_publisher("mavros/setpoint_raw/global", 1); + path_target_pub_ = this->create_publisher("mavros/trajectory/generated", 1); + vehicle_pose_pub_ = this->create_publisher("vehicle_pose_marker", 1); + camera_pose_pub_ = this->create_publisher("camera_pose_marker", 1); + planner_status_pub_ = this->create_publisher("planner_status", 1); + viewpoint_pub_ = this->create_publisher("viewpoints", 1); + planned_viewpoint_pub_ = this->create_publisher("planned_viewpoints", 1); + path_segment_pub_ = this->create_publisher("path_segments", 1); + tree_pub_ = this->create_publisher("tree", 1); + + mavlocalpose_sub_ = this->create_subscription( + "mavros/local_position/pose", 1, + std::bind(&TerrainPlanner::mavLocalPoseCallback, this, _1)); + mavglobalpose_sub_ = this->create_subscription( + "mavros/global_position/global", 1, + std::bind(&TerrainPlanner::mavGlobalPoseCallback, this, _1)); + mavtwist_sub_ = this->create_subscription( + "mavros/local_position/velocity_local", 1, + std::bind(&TerrainPlanner::mavtwistCallback, this, _1)); + global_origin_sub_ = this->create_subscription( + "mavros/global_position/gp_origin", 1, + std::bind(&TerrainPlanner::mavGlobalOriginCallback, this, _1)); + image_captured_sub_ = this->create_subscription( + "mavros/camera/image_captured", 1, + std::bind(&TerrainPlanner::mavImageCapturedCallback, this, _1)); + + setlocation_serviceserver_ = this->create_service( + "/terrain_planner/set_location", + std::bind(&TerrainPlanner::setLocationCallback, this, _1, _2)); + setmaxaltitude_serviceserver_ = this->create_service( + "/terrain_planner/set_max_altitude", + std::bind(&TerrainPlanner::setMaxAltitudeCallback, this, _1, _2)); + setgoal_serviceserver_ = this->create_service( + "/terrain_planner/set_goal", + std::bind(&TerrainPlanner::setGoalCallback, this, _1, _2)); + setstart_serviceserver_ = this->create_service( + "/terrain_planner/set_start", + std::bind(&TerrainPlanner::setStartCallback, this, _1, _2)); + setcurrentsegment_serviceserver_ = this->create_service( + "/terrain_planner/set_current_segment", + std::bind(&TerrainPlanner::setCurrentSegmentCallback, this, _1, _2)); + setstartloiter_serviceserver_ = this->create_service( + "/terrain_planner/set_start_loiter", + std::bind(&TerrainPlanner::setStartLoiterCallback, this, _1, _2)); + setplannerstate_service_server_ = this->create_service( + "/terrain_planner/set_planner_state", + std::bind(&TerrainPlanner::setPlannerStateCallback, this, _1, _2)); + setplanning_serviceserver_ = this->create_service( + "/terrain_planner/trigger_planning", + std::bind(&TerrainPlanner::setPlanningCallback, this, _1, _2)); + updatepath_serviceserver_ = this->create_service( + "/terrain_planner/set_path", + std::bind(&TerrainPlanner::setPathCallback, this, _1, _2)); + + msginterval_serviceclient_ = this->create_client( + "mavros/cmd/command"); std::string avalanche_map_path; - nh_private.param("terrain_path", map_path_, "resources/cadastre.tif"); - nh_private.param("terrain_color_path", map_color_path_, ""); - nh_private.param("resource_path", resource_path_, "resources"); - nh_private.param("meshresource_path", mesh_resource_path_, "../resources/believer.dae"); - nh_private.param("avalanche_map_path", avalanche_map_path, "../data/believer.dae"); - nh_private.param("minimum_turn_radius", goal_radius_, 66.67); + + map_path_ = this->declare_parameter("terrain_path", "resources/cadastre.tif"); + map_color_path_ = this->declare_parameter("terrain_color_path", ""); + resource_path_ = this->declare_parameter("resource_path", "resources"); + mesh_resource_path_ = this->declare_parameter("meshresource_path", "../resources/believer.dae"); + avalanche_map_path = this->declare_parameter("avalanche_map_path", "../data/believer.dae"); + goal_radius_ = this->declare_parameter("minimum_turn_radius", 66.67); terrain_map_ = std::make_shared(); @@ -129,41 +166,77 @@ TerrainPlanner::TerrainPlanner(const ros::NodeHandle &nh, const ros::NodeHandle global_planner_->setMap(terrain_map_); global_planner_->setAltitudeLimits(max_elevation_, min_elevation_); - // f = boost::bind(&TerrainPlanner::dynamicReconfigureCallback, this, _1, _2); - // server.setCallback(f); - planner_profiler_ = std::make_shared("planner"); } -TerrainPlanner::~TerrainPlanner() { - // Destructor + +void TerrainPlanner::init() { + //! @todo(srmainwaring) needs to be multi-threaded and use separate executors? + + // double plannerloop_dt_ = 2.0; + // ros::TimerOptions plannerlooptimer_options( + // ros::Duration(plannerloop_dt_), + // boost::bind(&TerrainPlanner::plannerloopCallback, this, _1), + // &plannerloop_queue_); + // plannerloop_timer_ = nh_.createTimer(plannerlooptimer_options); // Define timer for constant loop rate + // + // plannerloop_spinner_.reset(new ros::AsyncSpinner(1, &plannerloop_queue_)); + // plannerloop_spinner_->start(); + + auto plannerloop_dt_ = 2s; + plannerloop_timer_ = this->create_wall_timer( + plannerloop_dt_, std::bind(&TerrainPlanner::plannerloopCallback, this)); + // plannerloop_executor_.add_node(plannerloop_node_); + // plannerloop_executor_.spin(); + + // double statusloop_dt_ = 0.5; + // ros::TimerOptions statuslooptimer_options( + // ros::Duration(statusloop_dt_), boost::bind(&TerrainPlanner::statusloopCallback, this, _1), &statusloop_queue_); + // statusloop_timer_ = nh_.createTimer(statuslooptimer_options); // Define timer for constant loop rate + // + // statusloop_spinner_.reset(new ros::AsyncSpinner(1, &statusloop_queue_)); + // statusloop_spinner_->start(); + + auto statusloop_dt_ = 500ms; + statusloop_timer_ = this->create_wall_timer( + statusloop_dt_, std::bind(&TerrainPlanner::statusloopCallback, this)); + // statusloop_executor_.add_node(statusloop_node_); + // statusloop_executor_.spin(); + + // double cmdloop_dt_ = 0.1; + // ros::TimerOptions cmdlooptimer_options(ros::Duration(cmdloop_dt_), + // boost::bind(&TerrainPlanner::cmdloopCallback, this, _1), &cmdloop_queue_); + // cmdloop_timer_ = nh_.createTimer(cmdlooptimer_options); // Define timer for constant loop rate + // + // cmdloop_spinner_.reset(new ros::AsyncSpinner(1, &cmdloop_queue_)); + // cmdloop_spinner_->start(); + + auto cmdloop_dt_ = 100ms; + cmdloop_timer_ = this->create_wall_timer( + cmdloop_dt_, std::bind(&TerrainPlanner::cmdloopCallback, this)); + // cmdloop_executor_.add_node(cmdloop_node_); + // cmdloop_executor_.spin(); } -void TerrainPlanner::Init() { - double plannerloop_dt_ = 2.0; - ros::TimerOptions plannerlooptimer_options( - ros::Duration(plannerloop_dt_), boost::bind(&TerrainPlanner::plannerloopCallback, this, _1), &plannerloop_queue_); - plannerloop_timer_ = nh_.createTimer(plannerlooptimer_options); // Define timer for constant loop rate - - plannerloop_spinner_.reset(new ros::AsyncSpinner(1, &plannerloop_queue_)); - plannerloop_spinner_->start(); - - double statusloop_dt_ = 0.5; - ros::TimerOptions statuslooptimer_options( - ros::Duration(statusloop_dt_), boost::bind(&TerrainPlanner::statusloopCallback, this, _1), &statusloop_queue_); - statusloop_timer_ = nh_.createTimer(statuslooptimer_options); // Define timer for constant loop rate - - statusloop_spinner_.reset(new ros::AsyncSpinner(1, &statusloop_queue_)); - statusloop_spinner_->start(); - double cmdloop_dt_ = 0.1; - ros::TimerOptions cmdlooptimer_options(ros::Duration(cmdloop_dt_), - boost::bind(&TerrainPlanner::cmdloopCallback, this, _1), &cmdloop_queue_); - cmdloop_timer_ = nh_.createTimer(cmdlooptimer_options); // Define timer for constant loop rate - - cmdloop_spinner_.reset(new ros::AsyncSpinner(1, &cmdloop_queue_)); - cmdloop_spinner_->start(); +Eigen::Vector4d TerrainPlanner::rpy2quaternion(double roll, double pitch, double yaw) { + double cy = std::cos(yaw * 0.5); + double sy = std::sin(yaw * 0.5); + double cp = std::cos(pitch * 0.5); + double sp = std::sin(pitch * 0.5); + double cr = std::cos(roll * 0.5); + double sr = std::sin(roll * 0.5); + + Eigen::Vector4d q; + q(0) = cr * cp * cy + sr * sp * sy; + q(1) = sr * cp * cy - cr * sp * sy; + q(2) = cr * sp * cy + sr * cp * sy; + q(3) = cr * cp * sy - sr * sp * cy; + + q.normalize(); + + return q; } -void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { +void TerrainPlanner::cmdloopCallback() { if (!map_initialized_) return; if (!reference_primitive_.segments.empty()) { @@ -197,7 +270,7 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { /// Blend curvature with next segment double curvature_reference = reference_curvature; int current_segment_idx = reference_primitive_.getCurrentSegmentIndex(vehicle_position_); - bool is_last_segment = bool(current_segment_idx >= (reference_primitive_.segments.size() - 1)); + bool is_last_segment = bool(current_segment_idx >= static_cast(reference_primitive_.segments.size() - 1)); if (!is_last_segment) { /// Get next segment curvature double next_segment_curvature = reference_primitive_.segments[current_segment_idx + 1].curvature; @@ -219,7 +292,7 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { tracking_error_ = reference_position - vehicle_position_; planner_enabled_ = true; - if ((ros::Time::now() - last_triggered_time_).toSec() > 2.0 && planner_mode_ == PLANNER_MODE::ACTIVE_MAPPING) { + if ((this->get_clock()->now() - last_triggered_time_).nanoseconds() * 1.0E9 > 2.0 && planner_mode_ == PLANNER_MODE::ACTIVE_MAPPING) { bool dummy_camera = false; if (dummy_camera) { const int id = viewpoints_.size() + added_viewpoint_list.size(); @@ -228,16 +301,32 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { } else { /// TODO: Trigger camera when viewpoint reached /// This can be done using the mavlink message MAV_CMD_IMAGE_START_CAPTURE - mavros_msgs::CommandLong image_capture_msg; - image_capture_msg.request.command = mavros_msgs::CommandCode::DO_DIGICAM_CONTROL; - image_capture_msg.request.param1 = 0; // id - image_capture_msg.request.param2 = 0; // interval - image_capture_msg.request.param3 = 0; // total images - image_capture_msg.request.param4 = 0; // sequence number - image_capture_msg.request.param5 = 1; // sequence number - image_capture_msg.request.param6 = 0; // sequence number - image_capture_msg.request.param7 = 0; // sequence number - msginterval_serviceclient_.call(image_capture_msg); + auto image_capture_req = std::make_shared(); + image_capture_req->command = mavros_msgs::msg::CommandCode::DO_DIGICAM_CONTROL; + image_capture_req->param1 = 0; // id + image_capture_req->param2 = 0; // interval + image_capture_req->param3 = 0; // total images + image_capture_req->param4 = 0; // sequence number + image_capture_req->param5 = 1; // sequence number + image_capture_req->param6 = 0; // sequence number + image_capture_req->param7 = 0; // sequence number + + while (!msginterval_serviceclient_->wait_for_service(1s)) { + RCLCPP_WARN_STREAM(this->get_logger(), "Service [" + << msginterval_serviceclient_->get_service_name() + << "] not available."); + return; + } + + auto result = msginterval_serviceclient_->async_send_request(image_capture_req); + + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" + << msginterval_serviceclient_->get_service_name() + << "] failed."); + return; + } } /// TODO: Get reference attitude from path reference states const double pitch = std::atan(reference_tangent(2) / reference_tangent.head(2).norm()); @@ -247,7 +336,7 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { const int id = viewpoints_.size() + planned_viewpoint_list.size(); ViewPoint viewpoint(id, reference_position, reference_attitude); planned_viewpoint_list.push_back(viewpoint); - last_triggered_time_ = ros::Time::now(); + last_triggered_time_ = this->get_clock()->now(); } } else { tracking_error_ = Eigen::Vector3d::Zero(); @@ -255,15 +344,15 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { } } - planner_msgs::NavigationStatus msg; - msg.header.stamp = ros::Time::now(); + planner_msgs::msg::NavigationStatus msg; + msg.header.stamp = this->get_clock()->now(); // msg.planner_time.data = planner_time; msg.state = static_cast(planner_state_); msg.tracking_error = toVector3(tracking_error_); msg.enabled = planner_enabled_; // msg.reference_position = toVector3(reference_position); msg.vehicle_position = toVector3(vehicle_position_); - planner_status_pub_.publish(msg); + planner_status_pub_->publish(msg); publishVehiclePose(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_, mesh_resource_path_); publishVelocityMarker(vehicle_velocity_pub_, vehicle_position_, vehicle_velocity_); @@ -271,26 +360,7 @@ void TerrainPlanner::cmdloopCallback(const ros::TimerEvent &event) { publishCameraView(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_); } -Eigen::Vector4d TerrainPlanner::rpy2quaternion(double roll, double pitch, double yaw) { - double cy = std::cos(yaw * 0.5); - double sy = std::sin(yaw * 0.5); - double cp = std::cos(pitch * 0.5); - double sp = std::sin(pitch * 0.5); - double cr = std::cos(roll * 0.5); - double sr = std::sin(roll * 0.5); - - Eigen::Vector4d q; - q(0) = cr * cp * cy + sr * sp * sy; - q(1) = sr * cp * cy - cr * sp * sy; - q(2) = cr * sp * cy + sr * cp * sy; - q(3) = cr * cp * sy - sr * sp * cy; - - q.normalize(); - - return q; -} - -void TerrainPlanner::statusloopCallback(const ros::TimerEvent &event) { +void TerrainPlanner::statusloopCallback() { // Check if we want to update the planner state if query state and current state is different planner_state_ = finiteStateMachine(planner_state_, query_planner_state_); if (query_planner_state_ != planner_state_) { // Query has been rejected, reset @@ -299,7 +369,7 @@ void TerrainPlanner::statusloopCallback(const ros::TimerEvent &event) { // printPlannerState(planner_state_); } -void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { +void TerrainPlanner::plannerloopCallback() { const std::lock_guard lock(goal_mutex_); if (local_origin_received_ && !map_initialized_) { std::cout << "[TerrainPlanner] Local origin received, loading map" << std::endl; @@ -326,10 +396,27 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { } if (!local_origin_received_) { std::cout << "Requesting global origin messages" << std::endl; - mavros_msgs::CommandLong request_global_origin_msg; - request_global_origin_msg.request.command = mavros_msgs::CommandCode::REQUEST_MESSAGE; - request_global_origin_msg.request.param1 = 49; - msginterval_serviceclient_.call(request_global_origin_msg); + + auto request_global_origin_req = std::make_shared(); + request_global_origin_req->command = mavros_msgs::msg::CommandCode::REQUEST_MESSAGE; + request_global_origin_req->param1 = 49; + + while (!msginterval_serviceclient_->wait_for_service(1s)) { + RCLCPP_WARN_STREAM(this->get_logger(), "Service [" + << msginterval_serviceclient_->get_service_name() + << "] not available."); + return; + } + + auto result = msginterval_serviceclient_->async_send_request(request_global_origin_req); + + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" + << msginterval_serviceclient_->get_service_name() + << "] failed."); + return; + } return; } @@ -338,9 +425,10 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { switch (planner_mode_) { case PLANNER_MODE::GLOBAL: { // Solve planning problem with RRT* - double time_spent_planning = ros::Duration(ros::Time::now() - plan_time_).toSec(); + double time_spent_planning = rclcpp::Duration(this->get_clock()->now() - plan_time_).nanoseconds() * 1.0E9; if (time_spent_planning < planner_time_budget_) { - bool found_solution = global_planner_->Solve(1.0, candidate_primitive_); + //bool found_solution = + global_planner_->Solve(1.0, candidate_primitive_); publishTree(tree_pub_, global_planner_->getPlannerData(), global_planner_->getProblemSetup()); } else { publishPathSegments(path_segment_pub_, candidate_primitive_); @@ -410,7 +498,7 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { /// TODO: Figure out which rally point the planner is using double min_distance_error = std::numeric_limits::infinity(); int min_distance_index = -1; - for (int idx = 0; idx < rally_points.size(); idx++) { + for (int idx = 0; idx < static_cast(rally_points.size()); idx++) { double radial_error = std::abs((end_position - rally_points[idx]).norm() - dubins_state_space_->getMinTurningRadius()); if (radial_error < min_distance_error) { @@ -420,9 +508,9 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { } Eigen::Vector3d radial_vector = (end_position - rally_points[min_distance_index]); radial_vector(2) = 0.0; // Only consider horizontal loiters - Eigen::Vector3d emergency_rates = - 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); - double horizon = 2 * M_PI / std::abs(emergency_rates(2)); + //Eigen::Vector3d emergency_rates = + // 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); + //double horizon = 2 * M_PI / std::abs(emergency_rates(2)); // Append a loiter at the end of the planned path PathSegment loiter_trajectory; generateCircle(end_position, end_velocity, rally_points[min_distance_index], loiter_trajectory); @@ -478,9 +566,9 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { Eigen::Vector3d radial_vector = (end_position - home_position_); radial_vector(2) = 0.0; // Only consider horizontal loiters - Eigen::Vector3d emergency_rates = - 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); - double horizon = 2 * M_PI / std::abs(emergency_rates(2)); + // Eigen::Vector3d emergency_rates = + // 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); + //double horizon = 2 * M_PI / std::abs(emergency_rates(2)); // Append a loiter at the end of the planned path PathSegment loiter_trajectory; generateCircle(end_position, end_velocity, home_position_, loiter_trajectory); @@ -493,7 +581,10 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) { } publishPathSegments(path_segment_pub_, candidate_primitive_); break; + } + default: + break; } // double planner_time = planner_profiler_->toc(); @@ -511,7 +602,7 @@ PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_sta case PLANNER_STATE::NAVIGATE: { // Switch to Hold when segment has been completed int current_segment_idx = reference_primitive_.getCurrentSegmentIndex(vehicle_position_); - bool is_last_segment = bool(current_segment_idx >= (reference_primitive_.segments.size() - 1)); + bool is_last_segment = bool(current_segment_idx >= static_cast(reference_primitive_.segments.size() - 1)); if (is_last_segment) { /// TODO: Clear candidate primitive candidate_primitive_.segments.clear(); @@ -580,6 +671,8 @@ PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_sta planner_mode_ = PLANNER_MODE::ACTIVE_MAPPING; next_state = query_state; break; + default: + break; } } break; @@ -588,7 +681,7 @@ PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_sta /// TODO: Check if we have a return position defined // Switch to Hold when segment has been completed int current_segment_idx = reference_primitive_.getCurrentSegmentIndex(vehicle_position_); - bool is_last_segment = bool(current_segment_idx >= (reference_primitive_.segments.size() - 1)); + bool is_last_segment = bool(current_segment_idx >= static_cast(reference_primitive_.segments.size() - 1)); if (is_last_segment) { /// TODO: Clear candidate primitive candidate_primitive_.segments.clear(); @@ -607,7 +700,7 @@ PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_sta } case PLANNER_STATE::ABORT: { int current_segment_idx = reference_primitive_.getCurrentSegmentIndex(vehicle_position_); - bool is_last_segment = bool(current_segment_idx >= (reference_primitive_.segments.size() - 1)); + bool is_last_segment = bool(current_segment_idx >= static_cast(reference_primitive_.segments.size() - 1)); if (is_last_segment) { /// TODO: Clear candidate primitive candidate_primitive_.segments.clear(); @@ -621,26 +714,26 @@ PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_sta } void TerrainPlanner::publishTrajectory(std::vector trajectory) { - nav_msgs::Path msg; - std::vector posestampedhistory_vector; + nav_msgs::msg::Path msg; + std::vector posestampedhistory_vector; Eigen::Vector4d orientation(1.0, 0.0, 0.0, 0.0); for (auto pos : trajectory) { posestampedhistory_vector.insert(posestampedhistory_vector.begin(), vector3d2PoseStampedMsg(pos, orientation)); } - msg.header.stamp = ros::Time::now(); + msg.header.stamp = this->get_clock()->now(); msg.header.frame_id = "map"; msg.poses = posestampedhistory_vector; - vehicle_path_pub_.publish(msg); + vehicle_path_pub_->publish(msg); } -void TerrainPlanner::mavLocalPoseCallback(const geometry_msgs::PoseStamped &msg) { +void TerrainPlanner::mavLocalPoseCallback(const geometry_msgs::msg::PoseStamped &msg) { vehicle_attitude_(0) = msg.pose.orientation.w; vehicle_attitude_(1) = msg.pose.orientation.x; vehicle_attitude_(2) = msg.pose.orientation.y; vehicle_attitude_(3) = msg.pose.orientation.z; } -void TerrainPlanner::mavGlobalPoseCallback(const sensor_msgs::NavSatFix &msg) { +void TerrainPlanner::mavGlobalPoseCallback(const sensor_msgs::msg::NavSatFix &msg) { Eigen::Vector3d wgs84_vehicle_position; wgs84_vehicle_position(0) = msg.latitude; wgs84_vehicle_position(1) = msg.longitude; @@ -662,19 +755,18 @@ void TerrainPlanner::mavGlobalPoseCallback(const sensor_msgs::NavSatFix &msg) { vehicle_position_ = transformed_coordinates - map_origin; } -void TerrainPlanner::mavtwistCallback(const geometry_msgs::TwistStamped &msg) { +void TerrainPlanner::mavtwistCallback(const geometry_msgs::msg::TwistStamped &msg) { vehicle_velocity_ = toEigen(msg.twist.linear); // mavRate_ = toEigen(msg.twist.angular); } -void TerrainPlanner::MapPublishOnce(const ros::Publisher &pub, const grid_map::GridMap &map) { - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(map, message); - pub.publish(message); +void TerrainPlanner::MapPublishOnce(rclcpp::Publisher::SharedPtr pub, const grid_map::GridMap &map) { + auto message = grid_map::GridMapRosConverter::toMessage(map); + pub->publish(*message); } -void TerrainPlanner::publishPositionHistory(ros::Publisher &pub, const Eigen::Vector3d &position, - std::vector &history_vector) { +void TerrainPlanner::publishPositionHistory(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, + std::vector &history_vector) { unsigned int posehistory_window_ = 200; Eigen::Vector4d vehicle_attitude(1.0, 0.0, 0.0, 0.0); history_vector.insert(history_vector.begin(), vector3d2PoseStampedMsg(position, vehicle_attitude)); @@ -682,22 +774,22 @@ void TerrainPlanner::publishPositionHistory(ros::Publisher &pub, const Eigen::Ve history_vector.pop_back(); } - nav_msgs::Path msg; - msg.header.stamp = ros::Time::now(); + nav_msgs::msg::Path msg; + msg.header.stamp = this->get_clock()->now(); msg.header.frame_id = "map"; msg.poses = history_vector; - pub.publish(msg); + pub->publish(msg); } -void TerrainPlanner::publishGlobalPositionSetpoints(const ros::Publisher &pub, const double latitude, +void TerrainPlanner::publishGlobalPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const double latitude, const double longitude, const double altitude, const Eigen::Vector3d &velocity, const double curvature) { using namespace mavros_msgs; // Publishes position setpoints sequentially as trajectory setpoints - mavros_msgs::GlobalPositionTarget msg; - msg.header.stamp = ros::Time::now(); - msg.coordinate_frame = GlobalPositionTarget::FRAME_GLOBAL_REL_ALT; + mavros_msgs::msg::GlobalPositionTarget msg; + msg.header.stamp = this->get_clock()->now(); + msg.coordinate_frame = mavros_msgs::msg::GlobalPositionTarget::FRAME_GLOBAL_REL_ALT; msg.type_mask = 0.0; msg.latitude = latitude; msg.longitude = longitude; @@ -712,16 +804,16 @@ void TerrainPlanner::publishGlobalPositionSetpoints(const ros::Publisher &pub, c msg.acceleration_or_force.y = lateral_acceleration(1); msg.acceleration_or_force.z = lateral_acceleration(2); - pub.publish(msg); + pub->publish(msg); } -void TerrainPlanner::publishPositionSetpoints(const ros::Publisher &pub, const Eigen::Vector3d &position, +void TerrainPlanner::publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, const double curvature) { using namespace mavros_msgs; // Publishes position setpoints sequentially as trajectory setpoints - mavros_msgs::PositionTarget msg; - msg.header.stamp = ros::Time::now(); - msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED; + mavros_msgs::msg::PositionTarget msg; + msg.header.stamp = this->get_clock()->now(); + msg.coordinate_frame = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED; msg.type_mask = 0.0; msg.position.x = position(0); msg.position.y = position(1); @@ -736,35 +828,35 @@ void TerrainPlanner::publishPositionSetpoints(const ros::Publisher &pub, const E msg.acceleration_or_force.y = lateral_acceleration(1); msg.acceleration_or_force.z = lateral_acceleration(2); - pub.publish(msg); + pub->publish(msg); } -void TerrainPlanner::publishVelocityMarker(const ros::Publisher &pub, const Eigen::Vector3d &position, +void TerrainPlanner::publishVelocityMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const Eigen::Vector3d &velocity) { - visualization_msgs::Marker marker = vector2ArrowsMsg(position, velocity, 0, Eigen::Vector3d(1.0, 0.0, 1.0)); - pub.publish(marker); + visualization_msgs::msg::Marker marker = vector2ArrowsMsg(position, velocity, 0, Eigen::Vector3d(1.0, 0.0, 1.0)); + pub->publish(marker); } -void TerrainPlanner::publishReferenceMarker(const ros::Publisher &pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity, const double curvature) { +void TerrainPlanner::publishReferenceMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, + const Eigen::Vector3d &velocity, const double /*curvature*/) { Eigen::Vector3d scaled_velocity = 20.0 * velocity; - visualization_msgs::Marker marker = + visualization_msgs::msg::Marker marker = vector2ArrowsMsg(position, scaled_velocity, 0, Eigen::Vector3d(0.0, 0.0, 1.0), "reference"); - pub.publish(marker); + pub->publish(marker); } -void TerrainPlanner::publishPathSegments(ros::Publisher &pub, Path &trajectory) { - visualization_msgs::MarkerArray msg; +void TerrainPlanner::publishPathSegments(rclcpp::Publisher::SharedPtr pub, Path &trajectory) { + visualization_msgs::msg::MarkerArray msg; - std::vector marker; - visualization_msgs::Marker mark; - mark.action = visualization_msgs::Marker::DELETEALL; + std::vector marker; + visualization_msgs::msg::Marker mark; + mark.action = visualization_msgs::msg::Marker::DELETEALL; marker.push_back(mark); msg.markers = marker; - pub.publish(msg); + pub->publish(msg); - std::vector segment_markers; + std::vector segment_markers; int i = 0; for (auto &segment : trajectory.segments) { Eigen::Vector3d color = Eigen::Vector3d(1.0, 0.0, 0.0); @@ -778,46 +870,46 @@ void TerrainPlanner::publishPathSegments(ros::Publisher &pub, Path &trajectory) segment_markers.insert(segment_markers.begin(), point2MarkerMsg(segment.position().back(), i++, color)); } msg.markers = segment_markers; - pub.publish(msg); + pub->publish(msg); } -void TerrainPlanner::publishGoal(const ros::Publisher &pub, const Eigen::Vector3d &position, const double radius, +void TerrainPlanner::publishGoal(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const double radius, Eigen::Vector3d color, std::string name_space) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker = getGoalMarker(1, position, radius, color); marker.ns = name_space; - pub.publish(marker); + pub->publish(marker); } -void TerrainPlanner::publishRallyPoints(const ros::Publisher &pub, const std::vector &positions, +void TerrainPlanner::publishRallyPoints(rclcpp::Publisher::SharedPtr pub, const std::vector &positions, const double radius, Eigen::Vector3d color, std::string name_space) { - visualization_msgs::MarkerArray marker_array; - std::vector markers; + visualization_msgs::msg::MarkerArray marker_array; + std::vector markers; int marker_id = 1; for (const auto &position : positions) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker = getGoalMarker(marker_id, position, radius, color); marker.ns = name_space; markers.push_back(marker); marker_id++; } marker_array.markers = markers; - pub.publish(marker_array); + pub->publish(marker_array); } -visualization_msgs::Marker TerrainPlanner::getGoalMarker(const int id, const Eigen::Vector3d &position, +visualization_msgs::msg::Marker TerrainPlanner::getGoalMarker(const int id, const Eigen::Vector3d &position, const double radius, const Eigen::Vector3d color) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = this->get_clock()->now(); marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; /// TODO: Generate circular path double delta_theta = 0.05 * 2 * M_PI; for (double theta = 0.0; theta < 2 * M_PI + delta_theta; theta += delta_theta) { - geometry_msgs::Point point; + geometry_msgs::msg::Point point; point.x = position(0) + radius * std::cos(theta); point.y = position(1) + radius * std::sin(theta); point.z = position(2); @@ -838,15 +930,17 @@ visualization_msgs::Marker TerrainPlanner::getGoalMarker(const int id, const Eig return marker; } -void TerrainPlanner::mavstateCallback(const mavros_msgs::State::ConstPtr &msg) { current_state_ = *msg; } +void TerrainPlanner::mavstateCallback(const mavros_msgs::msg::State &msg) { + current_state_ = msg; +} void TerrainPlanner::publishPathSetpoints(const Eigen::Vector3d &position, const Eigen::Vector3d &velocity) { using namespace mavros_msgs; // Publishes position setpoints sequentially as trajectory setpoints - mavros_msgs::PositionTarget msg; - msg.header.stamp = ros::Time::now(); - msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED; - msg.type_mask = PositionTarget::IGNORE_AFX | PositionTarget::IGNORE_AFY | PositionTarget::IGNORE_AFZ; + mavros_msgs::msg::PositionTarget msg; + msg.header.stamp = this->get_clock()->now(); + msg.coordinate_frame = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED; + msg.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_AFX | mavros_msgs::msg::PositionTarget::IGNORE_AFY | mavros_msgs::msg::PositionTarget::IGNORE_AFZ; msg.position.x = position(0); msg.position.y = position(1); msg.position.z = position(2); @@ -856,9 +950,9 @@ void TerrainPlanner::publishPathSetpoints(const Eigen::Vector3d &position, const /// TODO: Package trajectory segments into trajectory waypoints - mavros_msgs::Trajectory trajectory_msg; - trajectory_msg.header.stamp = ros::Time::now(); - trajectory_msg.type = mavros_msgs::Trajectory::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; + mavros_msgs::msg::Trajectory trajectory_msg; + trajectory_msg.header.stamp = this->get_clock()->now(); + trajectory_msg.type = mavros_msgs::msg::Trajectory::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; trajectory_msg.point_valid[0] = true; trajectory_msg.point_valid[1] = true; trajectory_msg.point_1 = msg; @@ -867,17 +961,17 @@ void TerrainPlanner::publishPathSetpoints(const Eigen::Vector3d &position, const trajectory_msg.point_4 = msg; trajectory_msg.point_5 = msg; - path_target_pub_.publish(trajectory_msg); + path_target_pub_->publish(trajectory_msg); } -void TerrainPlanner::mavGlobalOriginCallback(const geographic_msgs::GeoPointStampedConstPtr &msg) { +void TerrainPlanner::mavGlobalOriginCallback(const geographic_msgs::msg::GeoPointStamped &msg) { std::cout << "[TerrainPlanner] Received Global Origin from FMU" << std::endl; local_origin_received_ = true; - double X = static_cast(msg->position.latitude); - double Y = static_cast(msg->position.longitude); - double Z = static_cast(msg->position.altitude); + double X = static_cast(msg.position.latitude); + double Y = static_cast(msg.position.longitude); + double Z = static_cast(msg.position.altitude); GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); double lat, lon, alt; earth.Reverse(X, Y, Z, lat, lon, alt); @@ -887,10 +981,10 @@ void TerrainPlanner::mavGlobalOriginCallback(const geographic_msgs::GeoPointStam local_origin_longitude_ = lon; } -void TerrainPlanner::mavMissionCallback(const mavros_msgs::WaypointListPtr &msg) { - for (auto &waypoint : msg->waypoints) { +void TerrainPlanner::mavMissionCallback(const mavros_msgs::msg::WaypointList &msg) { + for (auto &waypoint : msg.waypoints) { if (waypoint.is_current) { - if (waypoint.command == mavros_msgs::CommandCode::NAV_LOITER_UNLIM) { + if (waypoint.command == mavros_msgs::msg::CommandCode::NAV_LOITER_UNLIM) { // Get Loiter position std::cout << "NAV Loiter Center" << std::endl; std::cout << " - x_lat : " << waypoint.x_lat << std::endl; @@ -920,7 +1014,7 @@ void TerrainPlanner::mavMissionCallback(const mavros_msgs::WaypointListPtr &msg) } } -void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::CameraImageCaptured::ConstPtr &msg) { +void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured &/*msg*/) { // Publish recorded viewpoints /// TODO: Transform image tag into local position int id = viewpoints_.size(); @@ -929,10 +1023,11 @@ void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::CameraImageCapt // publishViewpoints(viewpoint_pub_, viewpoints_); } -bool TerrainPlanner::setLocationCallback(planner_msgs::SetString::Request &req, - planner_msgs::SetString::Response &res) { - std::string set_location = req.string; - bool align_location = req.align; +bool TerrainPlanner::setLocationCallback( + const std::shared_ptr req, + std::shared_ptr res) { + std::string set_location = req->string; + bool align_location = req->align; std::cout << "[TerrainPlanner] Set Location: " << set_location << std::endl; std::cout << "[TerrainPlanner] Set Alignment: " << align_location << std::endl; @@ -967,39 +1062,25 @@ bool TerrainPlanner::setLocationCallback(planner_msgs::SetString::Request &req, posehistory_vector_.clear(); MapPublishOnce(grid_map_pub_, terrain_map_->getGridMap()); } - res.success = result; + res->success = result; return true; } -// void TerrainPlanner::dynamicReconfigureCallback(terrain_navigation_ros::HeightRateTuningConfig &config, uint32_t -// level) -// { -// if (K_z_ != config.K_z) { -// K_z_ = config.K_z; -// ROS_INFO("Reconfigure request : K_z_ = %.2f ", config.K_z); -// } -// if (cruise_speed_ != config.cruise_speed) { -// cruise_speed_ = config.cruise_speed; -// ROS_INFO("Reconfigure request : cruise_speed_ = %.2f ", config.cruise_speed); -// } -// if (max_climb_rate_control_ != config.max_climb_rate) { -// max_climb_rate_control_ = config.max_climb_rate; -// ROS_INFO("Reconfigure request : max_climb_rate = %.2f ", config.max_climb_rate); -// } -// } - -bool TerrainPlanner::setMaxAltitudeCallback(planner_msgs::SetString::Request &req, - planner_msgs::SetString::Response &res) { - bool check_max_altitude = req.align; +bool TerrainPlanner::setMaxAltitudeCallback( + const std::shared_ptr req, + std::shared_ptr res) { + bool check_max_altitude = req->align; std::cout << "[TerrainPlanner] Max altitude constraint configured: " << check_max_altitude << std::endl; global_planner_->setMaxAltitudeCollisionChecks(check_max_altitude); - res.success = true; + res->success = true; return true; } -bool TerrainPlanner::setGoalCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res) { +bool TerrainPlanner::setGoalCallback( + const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); - Eigen::Vector3d candidate_goal = Eigen::Vector3d(req.vector.x, req.vector.y, req.vector.z); + Eigen::Vector3d candidate_goal = Eigen::Vector3d(req->vector.x, req->vector.y, req->vector.z); Eigen::Vector3d new_goal; std::cout << "[TerrainPlanner] Candidate goal: " << candidate_goal.transpose() << std::endl; bool is_goal_safe = validatePosition(terrain_map_->getGridMap(), candidate_goal, new_goal); @@ -1007,36 +1088,39 @@ bool TerrainPlanner::setGoalCallback(planner_msgs::SetVector3::Request &req, pla goal_pos_ = new_goal; // mcts_planner_->setGoal(new_goal); // problem_updated_ = true; - res.success = true; + res->success = true; publishGoal(candidate_goal_pub_, new_goal, goal_radius_, Eigen::Vector3d(0.0, 1.0, 0.0), "goal"); return true; } else { - res.success = false; + res->success = false; publishGoal(candidate_goal_pub_, new_goal, goal_radius_, Eigen::Vector3d(1.0, 0.0, 0.0), "goal"); return false; } } -bool TerrainPlanner::setStartCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res) { +bool TerrainPlanner::setStartCallback( + const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); - Eigen::Vector3d candidate_start = Eigen::Vector3d(req.vector.x, req.vector.y, req.vector.z); + Eigen::Vector3d candidate_start = Eigen::Vector3d(req->vector.x, req->vector.y, req->vector.z); Eigen::Vector3d new_start; std::cout << "[TerrainPlanner] Candidate start: " << candidate_start.transpose() << std::endl; bool is_safe = validatePosition(terrain_map_->getGridMap(), candidate_start, new_start); if (is_safe) { start_pos_ = new_start; - res.success = true; + res->success = true; publishGoal(candidate_start_pub_, new_start, start_loiter_radius_, Eigen::Vector3d(0.0, 1.0, 0.0), "start"); return true; } else { - res.success = false; + res->success = false; publishGoal(candidate_start_pub_, new_start, start_loiter_radius_, Eigen::Vector3d(1.0, 0.0, 0.0), "start"); return false; } } -bool TerrainPlanner::setCurrentSegmentCallback(planner_msgs::SetService::Request &req, - planner_msgs::SetService::Response &res) { +bool TerrainPlanner::setCurrentSegmentCallback( + const std::shared_ptr /*req*/, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); /// TODO: Get center of the last segment of the reference path if (!reference_primitive_.segments.empty()) { @@ -1054,11 +1138,11 @@ bool TerrainPlanner::setCurrentSegmentCallback(planner_msgs::SetService::Request start_pos_ = new_start; /// TODO: Curvature sign seems to be the opposite from mission items start_loiter_radius_ = -1 / last_segment.curvature; - res.success = true; + res->success = true; publishGoal(candidate_start_pub_, new_start, goal_radius_, Eigen::Vector3d(0.0, 1.0, 0.0), "start"); return true; } else { - res.success = false; + res->success = false; publishGoal(candidate_start_pub_, new_start, goal_radius_, Eigen::Vector3d(1.0, 0.0, 0.0), "start"); return false; } @@ -1067,12 +1151,13 @@ bool TerrainPlanner::setCurrentSegmentCallback(planner_msgs::SetService::Request } } std::cout << "[TerrainPlanner] Could not select current segment, reference is empty" << std::endl; - res.success = false; + res->success = false; return true; } -bool TerrainPlanner::setStartLoiterCallback(planner_msgs::SetService::Request &req, - planner_msgs::SetService::Response &res) { +bool TerrainPlanner::setStartLoiterCallback( + const std::shared_ptr /*req*/, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); std::cout << "[TerrainPlanner] Current Loiter start: " << mission_loiter_center_.transpose() << std::endl; Eigen::Vector3d new_start; @@ -1082,75 +1167,79 @@ bool TerrainPlanner::setStartLoiterCallback(planner_msgs::SetService::Request &r start_loiter_radius_ = mission_loiter_radius_; home_position_ = start_pos_; home_position_radius_ = start_loiter_radius_; - res.success = true; + res->success = true; publishGoal(candidate_start_pub_, start_pos_, std::abs(mission_loiter_radius_), Eigen::Vector3d(0.0, 1.0, 0.0), "start"); return true; } else { - res.success = false; + res->success = false; publishGoal(candidate_start_pub_, start_pos_, std::abs(mission_loiter_radius_), Eigen::Vector3d(1.0, 0.0, 0.0), "start"); return false; } } -bool TerrainPlanner::setPlanningCallback(planner_msgs::SetVector3::Request &req, - planner_msgs::SetVector3::Response &res) { +bool TerrainPlanner::setPlanningCallback( + const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); - planner_time_budget_ = req.vector.z; + planner_time_budget_ = req->vector.z; std::cout << "[TerrainPlanner] Planning budget: " << planner_time_budget_ << std::endl; problem_updated_ = true; - plan_time_ = ros::Time::now(); + plan_time_ = this->get_clock()->now(); global_planner_->setupProblem(start_pos_, goal_pos_, start_loiter_radius_); planner_mode_ = PLANNER_MODE::GLOBAL; - res.success = true; + res->success = true; return true; } -bool TerrainPlanner::setPathCallback(planner_msgs::SetVector3::Request &req, planner_msgs::SetVector3::Response &res) { +bool TerrainPlanner::setPathCallback( + const std::shared_ptr /*req*/, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); if (!candidate_primitive_.segments.empty()) { query_planner_state_ = PLANNER_STATE::NAVIGATE; - res.success = true; + res->success = true; } else { - res.success = false; + res->success = false; } return true; } -bool TerrainPlanner::setPlannerStateCallback(planner_msgs::SetPlannerState::Request &req, - planner_msgs::SetPlannerState::Response &res) { +bool TerrainPlanner::setPlannerStateCallback( + const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); - int planner_state = static_cast(req.state); + int planner_state = static_cast(req->state); switch (planner_state) { case (1): { query_planner_state_ = PLANNER_STATE::HOLD; - res.success = true; + res->success = true; break; } case (2): { query_planner_state_ = PLANNER_STATE::NAVIGATE; - res.success = true; + res->success = true; break; } case (3): { query_planner_state_ = PLANNER_STATE::ROLLOUT; - res.success = true; + res->success = true; break; } case (4): { query_planner_state_ = PLANNER_STATE::ABORT; - res.success = true; + res->success = true; break; } case (5): { query_planner_state_ = PLANNER_STATE::RETURN; - res.success = true; + res->success = true; break; } default: { - res.success = false; + res->success = false; break; } } diff --git a/terrain_navigation_ros/src/terrain_planner_node.cpp b/terrain_navigation_ros/src/terrain_planner_node.cpp index 1ede82d4..b709a0b1 100644 --- a/terrain_navigation_ros/src/terrain_planner_node.cpp +++ b/terrain_navigation_ros/src/terrain_planner_node.cpp @@ -37,26 +37,26 @@ * @author Jaeyoung Lim */ +#include + #include "terrain_navigation_ros/terrain_planner.h" int main(int argc, char **argv) { - ros::init(argc, argv, "terrain_planner"); - ros::NodeHandle nh(""); - ros::NodeHandle nh_private("~"); - - std::string file_path, output_file_path; - int num_experiments; - double max_experiment_duration; - nh_private.param("file_path", file_path, ""); - nh_private.param("num_experiments", num_experiments, 1); - nh_private.param("max_experiment_duration", max_experiment_duration, 500); - nh_private.param("output_file_path", output_file_path, "output/benchmark.csv"); + rclcpp::init(argc, argv); - std::shared_ptr terrain_planner_node = std::make_shared(nh, nh_private); + // std::string file_path, output_file_path; + // int num_experiments; + // double max_experiment_duration; + // nh_private.param("file_path", file_path, ""); + // nh_private.param("num_experiments", num_experiments, 1); + // nh_private.param("max_experiment_duration", max_experiment_duration, 500); + // nh_private.param("output_file_path", output_file_path, "output/benchmark.csv"); - // Initiate ROS spinners - terrain_planner_node->Init(); + auto terrain_planner_node = std::make_shared(); + terrain_planner_node->init(); - ros::spin(); + rclcpp::spin(terrain_planner_node); + rclcpp::shutdown(); return 0; + } diff --git a/terrain_planner/CMakeLists.txt b/terrain_planner/CMakeLists.txt index 44d36a46..4019c4ca 100644 --- a/terrain_planner/CMakeLists.txt +++ b/terrain_planner/CMakeLists.txt @@ -1,90 +1,197 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.14.4) project(terrain_planner) -add_definitions(-std=c++17 -Wall) +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Set policy for 3.16 for CMP0076 defaulting to ON +cmake_policy(VERSION 3.16) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_geo REQUIRED) +find_package(grid_map_cv REQUIRED) +# find_package(grid_map_pcl REQUIRED) +find_package(grid_map_ros REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(terrain_navigation REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(Boost REQUIRED COMPONENTS serialization system filesystem) +find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) find_package(ompl REQUIRED) -find_package(Boost REQUIRED COMPONENTS serialization system filesystem) -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - tf +find_package(PkgConfig REQUIRED) +pkg_check_modules(ODE REQUIRED ode) + +# Libraries +add_library(${PROJECT_NAME} + src/common.cpp + src/DubinsAirplane.cpp + src/DubinsPath.cpp + src/terrain_ompl_rrt.cpp + src/terrain_ompl.cpp + src/visualization.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIR} +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + ${Boost_LIBRARIES} + ${OMPL_LIBRARIES} + ${OpenCV_LIBRARIES} + ${tf2_geometry_msgs_TARGETS} + tf2_eigen::tf2_eigen +) + +target_compile_options(${PROJECT_NAME} PRIVATE + "-Wno-gnu-zero-variadic-macro-arguments" +) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC grid_map_core - grid_map_cv - grid_map_msgs - grid_map_ros - grid_map_pcl grid_map_geo terrain_navigation ) -catkin_package( - INCLUDE_DIRS include - LIBRARIES terrain_planner - CATKIN_DEPENDS roscpp rospy +# Executables +add_executable(test_rrt_node + src/test_rrt_node.cpp ) -############# -# LIBRARIES # -############# -include_directories( - include - ${Boost_INCLUDE_DIR} - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} - ${OMPL_INCLUDE_DIR} +target_compile_options(test_rrt_node PRIVATE + "-Wno-gnu-zero-variadic-macro-arguments" ) -add_library(${PROJECT_NAME} - src/terrain_ompl.cpp - src/terrain_ompl_rrt.cpp - src/DubinsPath.cpp - src/DubinsAirplane.cpp +target_link_directories(test_rrt_node PUBLIC + ${ODE_LIBRARY_DIRS} + ${yaml_cpp_vendor_LIBRARY_DIRS} ) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES} ${Boost_LIBRARIES}) -############ -# BINARIES # -############ -add_executable(test_rrt_node - src/test_rrt_node.cpp +target_link_libraries(test_rrt_node PUBLIC + ${PROJECT_NAME} + ${ODE_LIBRARIES} + ${OMPL_LIBRARIES} + ${OpenCV_LIBRARIES} + ${geometry_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + tf2_eigen::tf2_eigen ) -add_dependencies(test_rrt_node ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_rrt_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) add_executable(test_ompl_dubins src/test_ompl_dubins.cpp ) -add_dependencies(test_ompl_dubins ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_ompl_dubins ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) + +target_compile_options(test_ompl_dubins PRIVATE + "-Wno-gnu-zero-variadic-macro-arguments" +) + +target_link_directories(test_ompl_dubins PUBLIC + ${ODE_LIBRARY_DIRS} + ${yaml_cpp_vendor_LIBRARY_DIRS} +) + +target_link_libraries(test_ompl_dubins PUBLIC + ${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ${ODE_LIBRARIES} + ${OMPL_LIBRARIES} + ${geometry_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + tf2_eigen::tf2_eigen +) add_executable(test_ompl_dubins_to_circle src/test_ompl_dubins_to_circle.cpp ) -add_dependencies(test_ompl_dubins_to_circle ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(test_ompl_dubins_to_circle ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES}) - -########## -# EXPORT # -########## -# cs_install() -# cs_export() -############# -## Testing ## -############# - -if(CATKIN_ENABLE_TESTING) - # Add gtest based cpp test target and link libraries - catkin_add_gtest(${PROJECT_NAME}-test - test/main.cpp) - - if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${YAML_CPP_LIBRARIES} ${OpenCV_LIBRARIES}) - endif() +target_compile_options(test_ompl_dubins_to_circle PRIVATE + "-Wno-gnu-zero-variadic-macro-arguments" +) + +target_link_directories(test_ompl_dubins_to_circle PUBLIC + ${ODE_LIBRARY_DIRS} + ${yaml_cpp_vendor_LIBRARY_DIRS} +) + +target_link_libraries(test_ompl_dubins_to_circle PUBLIC + ${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ${ODE_LIBRARIES} + ${OMPL_LIBRARIES} + ${geometry_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + tf2_eigen::tf2_eigen +) + +# Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +install( + TARGETS + test_rrt_node + test_ompl_dubins + test_ompl_dubins_to_circle + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +# Test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}-test + test/main.cpp + ) + + target_link_directories(${PROJECT_NAME}-test PUBLIC + ${ODE_LIBRARY_DIRS} + ${yaml_cpp_vendor_LIBRARY_DIRS} + ) + + target_link_libraries(${PROJECT_NAME}-test + ${PROJECT_NAME} + ${ODE_LIBRARIES} + ) endif() + +ament_package() diff --git a/terrain_planner/include/terrain_planner/common.h b/terrain_planner/include/terrain_planner/common.h index e17a0bc1..b19a37d3 100644 --- a/terrain_planner/include/terrain_planner/common.h +++ b/terrain_planner/include/terrain_planner/common.h @@ -40,13 +40,17 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include + #include +#include +#include + const std::map>> colorMap{ {"viridis", {{0.267004, 0.004874, 0.329415}, {0.268510, 0.009605, 0.335427}, {0.269944, 0.014625, 0.341379}, @@ -394,436 +398,51 @@ const std::map>> colorMap{ {1.000000, 0.000000, 0.813939}, {1.000000, 0.000000, 0.792626}, {1.000000, 0.000000, 0.771313}, {1.000000, 0.000000, 0.750000}}}}; -Eigen::Vector3d toEigen(const geometry_msgs::Point &p) { - Eigen::Vector3d ev3(p.x, p.y, p.z); - return ev3; -} - -Eigen::Vector3d toEigen(const geometry_msgs::Vector3 &v3) { - Eigen::Vector3d ev3(v3.x, v3.y, v3.z); - return ev3; -} +Eigen::Vector3d toEigen(const geometry_msgs::msg::Point &p); -Eigen::Vector3d toEigen(const geometry_msgs::Pose &p) { - Eigen::Vector3d position(p.position.x, p.position.y, p.position.z); - return position; -} +Eigen::Vector3d toEigen(const geometry_msgs::msg::Vector3 &v3); -geometry_msgs::Vector3 toVector3(const Eigen::Vector3d &p) { - geometry_msgs::Vector3 vector; - vector.x = p(0); - vector.y = p(1); - vector.z = p(2); - return vector; -} +Eigen::Vector3d toEigen(const geometry_msgs::msg::Pose &p); -geometry_msgs::Pose vector3d2PoseMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation) { - geometry_msgs::Pose encode_msg; +geometry_msgs::msg::Vector3 toVector3(const Eigen::Vector3d &p); - encode_msg.orientation.w = orientation(0); - encode_msg.orientation.x = orientation(1); - encode_msg.orientation.y = orientation(2); - encode_msg.orientation.z = orientation(3); - encode_msg.position.x = position(0); - encode_msg.position.y = position(1); - encode_msg.position.z = position(2); - return encode_msg; -} +geometry_msgs::msg::Pose vector3d2PoseMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation); -geometry_msgs::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation) { - geometry_msgs::PoseStamped encode_msg; +geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation); - encode_msg.header.stamp = ros::Time::now(); - encode_msg.header.frame_id = "map"; - encode_msg.pose.orientation.w = orientation(0); - encode_msg.pose.orientation.x = orientation(1); - encode_msg.pose.orientation.y = orientation(2); - encode_msg.pose.orientation.z = orientation(3); - encode_msg.pose.position.x = position(0); - encode_msg.pose.position.y = position(1); - encode_msg.pose.position.z = position(2); - return encode_msg; -} +visualization_msgs::msg::Marker utility2MarkerMsg(const double utility, const Eigen::Vector3d position, int id); -visualization_msgs::Marker utility2MarkerMsg(const double utility, const Eigen::Vector3d position, int id) { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "my_namespace"; - marker.id = id; - marker.type = visualization_msgs::Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = position(0); - marker.pose.position.y = position(1); - marker.pose.position.z = position(2); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - double vis_utility; - if (utility < 1.0) { - vis_utility = 1.0 / 1; - } else { - vis_utility = utility / 1; - } - marker.scale.x = vis_utility; - marker.scale.y = vis_utility; - marker.scale.z = vis_utility; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - return marker; -} - -visualization_msgs::Marker vector2ArrowsMsg(const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, +visualization_msgs::msg::Marker vector2ArrowsMsg(const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0), - const std::string marker_namespace = "normals") { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = marker_namespace; - marker.id = id; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - geometry_msgs::Point head; - head.x = position(0); - head.y = position(1); - head.z = position(2); - points.push_back(head); - geometry_msgs::Point tail; - tail.x = position(0) + normal(0); - tail.y = position(1) + normal(1); - tail.z = position(2) + normal(2); - points.push_back(tail); - - marker.points = points; - marker.scale.x = std::min(normal.norm(), 1.0); - marker.scale.y = std::min(normal.norm(), 2.0); - marker.scale.z = 0.0; - marker.color.a = 1.0; - marker.color.r = color(0); - marker.color.g = color(1); - marker.color.b = color(2); - return marker; -} - -visualization_msgs::Marker trajectory2MarkerMsg(Path &trajectory, const int id) { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "normals"; - marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - for (auto position : trajectory.position()) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - } - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.5; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.a = 0.2; - if (!trajectory.valid()) { - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - } else { - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - } - return marker; -} - -visualization_msgs::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, - std::string color_map = "plasma") { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "normals"; - marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - double max_altitude = -std::numeric_limits::infinity(); - double min_altitude = std::numeric_limits::infinity(); - for (auto &position : trajectory) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - if (position(2) < min_altitude) { - min_altitude = position(2); - } - if (position(2) > max_altitude) { - max_altitude = position(2); - } - } - marker.points = points; - - std::vector colors; - for (auto &position : trajectory) { - double intensity = (position(2) - min_altitude) / (max_altitude - min_altitude); - intensity = std::min(intensity, 1.0); - intensity = std::max(intensity, 0.0); - - const std::vector> &ctable = colorMap.at(color_map); - - int idx = int(floor(intensity * 255)); - idx = std::min(idx, 255); - idx = std::max(idx, 0); - - // Get color from table - std::vector rgb = ctable.at(idx); - - std_msgs::ColorRGBA color; - color.r = rgb[0]; - color.g = rgb[1]; - color.b = rgb[2]; - color.a = 0.8; - colors.push_back(color); - } - marker.colors = colors; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 2.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - return marker; -} - -visualization_msgs::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, - std::vector &trajectory_color) { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "normals"; - marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - double max_altitude = -std::numeric_limits::infinity(); - double min_altitude = std::numeric_limits::infinity(); - for (auto &position : trajectory) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - if (position(2) < min_altitude) { - min_altitude = position(2); - } - if (position(2) > max_altitude) { - max_altitude = position(2); - } - } - marker.points = points; - - std::vector colors; - for (auto &code : trajectory_color) { - std_msgs::ColorRGBA color; - color.r = code[0]; - color.g = code[1]; - color.b = code[2]; - color.a = 0.8; - colors.push_back(color); - } - marker.colors = colors; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 2.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - return marker; -} - -visualization_msgs::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "normals"; - marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - for (auto &position : trajectory) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - } - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 2.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - marker.color.a = 0.8; - marker.color.r = color.x(); - marker.color.g = color.y(); - marker.color.b = color.z(); - - return marker; -} - -visualization_msgs::Marker trajectory2MarkerMsg(PathSegment &trajectory, const int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "normals"; - marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - for (auto &position : trajectory.position()) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - } - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 2.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - marker.color.a = 1.0; - marker.color.r = color.x(); - marker.color.g = color.y(); - marker.color.b = color.z(); - - return marker; -} - -visualization_msgs::Marker point2MarkerMsg(Eigen::Vector3d &position, const int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) { - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "normals"; - marker.id = id; - marker.type = visualization_msgs::Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - geometry_msgs::Point point; - marker.pose.position.x = position(0); - marker.pose.position.y = position(1); - marker.pose.position.z = position(2); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 2.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - marker.color.a = 0.5; - marker.color.r = color.x(); - marker.color.g = color.y(); - marker.color.b = color.z(); + const std::string marker_namespace = "normals"); - return marker; -} +visualization_msgs::msg::Marker trajectory2MarkerMsg(Path &trajectory, const int id); -double GetTimeInSeconds(std::string date_time) { - std::stringstream ss(date_time); - std::string tagged_time; - std::string tagged_date; +visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, + std::string color_map = "plasma"); - ss >> tagged_date >> tagged_time; +visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, + std::vector &trajectory_color); - std::stringstream ss_time(tagged_time); - std::vector time_hour; +visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); - while (ss_time.good()) { - std::string substr; - std::getline(ss_time, substr, ':'); - time_hour.push_back(substr); - } +visualization_msgs::msg::Marker trajectory2MarkerMsg(PathSegment &trajectory, const int id, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); - return 3600.0 * std::stof(time_hour[0]) + 60.0 * std::stof(time_hour[1]) + std::stoi(time_hour[2]); -} +visualization_msgs::msg::Marker point2MarkerMsg(Eigen::Vector3d &position, const int id, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); -double StringToGeoReference(std::string &exif_tag) { - std::stringstream ss(exif_tag); - std::vector result; - while (ss.good()) { - std::string substr; - std::getline(ss, substr, '('); - std::getline(ss, substr, ')'); - result.push_back(substr); - } - double output; - if (result.size() >= 4) { - /// TODO: Check precision and coordinate frame of this conversion - output = std::stod(result[0]) + 0.0166667 * std::stod(result[1]) + 0.000277778 * std::stod(result[2]); - } else { - output = std::stod(result[0]); - } - return output; -} +double GetTimeInSeconds(std::string date_time); -bool parseAttitudeFromText(std::string text_path, std::string image_file, Eigen::Vector4d &attitude) { - std::ifstream file(text_path); - std::string str; +double StringToGeoReference(std::string &exif_tag); - // Look for the image file name in the path - while (getline(file, str)) { - if (str.find(image_file) != std::string::npos) { - std::stringstream ss(str); - std::vector camera_pose; - camera_pose.resize(9); - // # IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME - ss >> camera_pose[0] >> camera_pose[1] >> camera_pose[2] >> camera_pose[3] >> camera_pose[4]; - attitude << std::stof(camera_pose[1]), std::stof(camera_pose[2]), std::stof(camera_pose[3]), - std::stof(camera_pose[4]); - return true; - } - } - return false; -} +bool parseAttitudeFromText(std::string text_path, std::string image_file, Eigen::Vector4d &attitude); -double getRandom(double min, double max) { - return std::abs(max - min) * static_cast(rand()) / static_cast(RAND_MAX) + std::min(max, min); -} +double getRandom(double min, double max); -Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p) { - Eigen::Vector4d quat; - quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2), - p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0); - return quat; -} +Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p); -bool validatePosition(const grid_map::GridMap &map, const Eigen::Vector3d goal, Eigen::Vector3d &valid_goal) { - double upper_surface = map.atPosition("ics_+", goal.head(2)); - double lower_surface = map.atPosition("ics_-", goal.head(2)); - const bool is_goal_valid = (upper_surface < lower_surface) ? true : false; - valid_goal(0) = goal(0); - valid_goal(1) = goal(1); - valid_goal(2) = (upper_surface + lower_surface) / 2.0; - return is_goal_valid; -} +bool validatePosition(const grid_map::GridMap &map, const Eigen::Vector3d goal, Eigen::Vector3d &valid_goal); #endif diff --git a/terrain_planner/include/terrain_planner/terrain_ompl.h b/terrain_planner/include/terrain_planner/terrain_ompl.h index 75631b29..f4bc7b26 100644 --- a/terrain_planner/include/terrain_planner/terrain_ompl.h +++ b/terrain_planner/include/terrain_planner/terrain_ompl.h @@ -88,7 +88,7 @@ class TerrainStateSampler : public base::StateSampler { // std::cout << "Sample Near" << std::endl; return; } - void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, double stdDev) override { + void sampleGaussian(ompl::base::State* /*state*/, const ompl::base::State* /*mean*/, double /*stdDev*/) override { // std::cout << "Sample Gaussian" << std::endl; return; } diff --git a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h index 19505a33..ab3965de 100644 --- a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h +++ b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h @@ -197,7 +197,7 @@ class TerrainOmplRrt { } private: - double minimum_turning_radius_{66.67}; + // double minimum_turning_radius_{66.67}; std::shared_ptr problem_setup_; std::shared_ptr map_; double min_altitude_{50.0}; diff --git a/terrain_planner/include/terrain_planner/visualization.h b/terrain_planner/include/terrain_planner/visualization.h index f49d2181..ae1d3180 100644 --- a/terrain_planner/include/terrain_planner/visualization.h +++ b/terrain_planner/include/terrain_planner/visualization.h @@ -1,215 +1,52 @@ #ifndef TERRAIN_PLANNER_VISUALIZATION_H #define TERRAIN_PLANNER_VISUALIZATION_H -#include "terrain_navigation/path.h" -#include "terrain_planner/common.h" -#include "terrain_planner/ompl_setup.h" +#include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include -#include #include -#include -#include -#include -#include - -void publishCandidateManeuvers(const ros::Publisher& pub, const std::vector& candidate_maneuvers, - bool visualize_invalid_trajectories = false) { - visualization_msgs::MarkerArray msg; - - std::vector marker; - visualization_msgs::Marker mark; - mark.action = visualization_msgs::Marker::DELETEALL; - marker.push_back(mark); - msg.markers = marker; - pub.publish(msg); - - std::vector maneuver_library_vector; - int i = 0; - for (auto maneuver : candidate_maneuvers) { - if (maneuver.valid() || visualize_invalid_trajectories) { - maneuver_library_vector.insert(maneuver_library_vector.begin(), trajectory2MarkerMsg(maneuver, i)); - } - i++; - } - msg.markers = maneuver_library_vector; - pub.publish(msg); -} - -void publishPositionSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, - const Eigen::Vector3d& velocity, - const Eigen::Vector3d scale = Eigen::Vector3d(10.0, 2.0, 2.0)) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); - marker.type = visualization_msgs::Marker::ARROW; - marker.header.frame_id = "map"; - marker.id = 0; - marker.action = visualization_msgs::Marker::DELETEALL; - pub.publish(marker); +#include +#include +#include +#include - marker.header.stamp = ros::Time::now(); - marker.action = visualization_msgs::Marker::ADD; - marker.scale.x = scale(0); - marker.scale.y = scale(1); - marker.scale.z = scale(2); - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - marker.pose.position = tf2::toMsg(position); - tf2::Quaternion q; - q.setRPY(0, 0, std::atan2(velocity.y(), velocity.x())); - marker.pose.orientation = tf2::toMsg(q); +#include +#include - pub.publish(marker); -} +#include -void publishPath(const ros::Publisher& pub, std::vector path, Eigen::Vector3d color) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.header.frame_id = "map"; - marker.id = 0; - marker.action = visualization_msgs::Marker::ADD; - std::vector points; - for (auto& position : path) { - geometry_msgs::Point point; - point.x = position(0); - point.y = position(1); - point.z = position(2); - points.push_back(point); - } - std::cout << "Points: " << points.size() << std::endl; - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 10.0; - marker.scale.y = 10.0; - marker.scale.z = 10.0; - marker.color.a = 0.8; - marker.color.r = color.x(); - marker.color.g = color.y(); - marker.color.b = color.z(); - pub.publish(marker); -} - -void publishTrajectory(ros::Publisher& pub, std::vector trajectory) { - nav_msgs::Path msg; - std::vector posestampedhistory_vector; - Eigen::Vector4d orientation(1.0, 0.0, 0.0, 0.0); - for (auto pos : trajectory) { - posestampedhistory_vector.insert(posestampedhistory_vector.begin(), vector3d2PoseStampedMsg(pos, orientation)); - } - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = "map"; - msg.poses = posestampedhistory_vector; - pub.publish(msg); -} - -void publishTree(const ros::Publisher& pub, std::shared_ptr planner_data, - std::shared_ptr problem_setup) { - visualization_msgs::MarkerArray marker_array; - std::vector marker; - - planner_data->decoupleFromPlanner(); - - // Create states, a marker and a list to store edges - ompl::base::ScopedState vertex(problem_setup->getSpaceInformation()); - ompl::base::ScopedState neighbor_vertex( - problem_setup->getSpaceInformation()); - size_t marker_idx{0}; - auto dubins_ss = std::make_shared(); - for (size_t i = 0; i < planner_data->numVertices(); i++) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time().now(); - marker.header.frame_id = "map"; - vertex = planner_data->getVertex(i).getState(); - marker.ns = "vertex"; - marker.id = marker_idx++; - marker.pose.position.x = vertex[0]; - marker.pose.position.y = vertex[1]; - marker.pose.position.z = vertex[2]; - marker.pose.orientation.w = std::cos(0.5 * vertex[3]); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = std::sin(0.5 * vertex[3]); - marker.scale.x = 10.0; - marker.scale.y = 2.0; - marker.scale.z = 2.0; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - marker_array.markers.push_back(marker); - - // allocate variables - std::vector edge_list; - int num_edges = planner_data->getEdges(i, edge_list); - if (num_edges > 0) { - for (unsigned int edge : edge_list) { - visualization_msgs::Marker edge_marker; - edge_marker.header.stamp = ros::Time().now(); - edge_marker.header.frame_id = "map"; - edge_marker.id = marker_idx++; - edge_marker.type = visualization_msgs::Marker::LINE_STRIP; - edge_marker.ns = "edge"; - neighbor_vertex = planner_data->getVertex(edge).getState(); - // points.push_back(toMsg(Eigen::Vector3d(vertex[0], vertex[1], vertex[2]))); - // points.push_back(toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); - ompl::base::State* state = dubins_ss->allocState(); - ompl::base::State* from = dubins_ss->allocState(); - from->as()->setX(vertex[0]); - from->as()->setY(vertex[1]); - from->as()->setZ(vertex[2]); - from->as()->setYaw(vertex[3]); +#include "terrain_planner/common.h" +#include "terrain_planner/ompl_setup.h" - ompl::base::State* to = dubins_ss->allocState(); - to->as()->setX(neighbor_vertex[0]); - to->as()->setY(neighbor_vertex[1]); - to->as()->setZ(neighbor_vertex[2]); - to->as()->setYaw(neighbor_vertex[3]); - if (dubins_ss->equalStates(from, to)) { - continue; - } - std::vector points; - for (double t = 0.0; t < 1.0; t += 0.02) { - dubins_ss->interpolate(from, to, t, state); - auto interpolated_state = - Eigen::Vector3d(state->as()->getX(), - state->as()->getY(), - state->as()->getZ()); - points.push_back(tf2::toMsg(interpolated_state)); - } - points.push_back(tf2::toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); - edge_marker.points = points; - edge_marker.action = visualization_msgs::Marker::ADD; - edge_marker.pose.orientation.w = 1.0; - edge_marker.pose.orientation.x = 0.0; - edge_marker.pose.orientation.y = 0.0; - edge_marker.pose.orientation.z = 0.0; - edge_marker.scale.x = 1.0; - edge_marker.scale.y = 1.0; - edge_marker.scale.z = 1.0; - edge_marker.color.a = 0.5; // Don't forget to set the alpha! - edge_marker.color.r = 1.0; - edge_marker.color.g = 1.0; - edge_marker.color.b = 0.0; - marker_array.markers.push_back(edge_marker); - } - } - } - pub.publish(marker_array); -} +void publishCandidateManeuvers( + rclcpp::Publisher::SharedPtr pub, + const std::vector& candidate_maneuvers, + bool visualize_invalid_trajectories = false); + +void publishPositionSetpoints( + rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d& position, + const Eigen::Vector3d& velocity, + const Eigen::Vector3d scale = Eigen::Vector3d(10.0, 2.0, 2.0)); + +void publishPath( + rclcpp::Publisher::SharedPtr pub, + std::vector path, + Eigen::Vector3d color); + +void publishTrajectory( + rclcpp::Publisher::SharedPtr pub, + std::vector trajectory); + +void publishTree( + rclcpp::Publisher::SharedPtr pub, + std::shared_ptr planner_data, + std::shared_ptr problem_setup); #endif diff --git a/terrain_planner/launch/config.rviz b/terrain_planner/launch/config.rviz index 044949f3..b0debb13 100644 --- a/terrain_planner/launch/config.rviz +++ b/terrain_planner/launch/config.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -12,21 +12,21 @@ Panels: - /GridMap4 Splitter Ratio: 0.5 Tree Height: 531 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Name: Time SyncMode: 0 SyncSource: "" @@ -48,7 +48,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1000 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -66,7 +66,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 0; 255; 255 Enabled: true Head Diameter: 0.30000001192092896 @@ -90,7 +90,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 0; 0; 255 Enabled: true Head Diameter: 0.30000001192092896 @@ -185,7 +185,7 @@ Visualization Manager: Use ColorMap: true Value: false - Alpha: 1 - Class: rviz/Axes + Class: rviz_default_plugins/Axes Enabled: false Length: 100 Name: Axes @@ -193,7 +193,7 @@ Visualization Manager: Reference Frame: map Show Trail: false Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /visualization_marker Name: MarkerArray @@ -201,7 +201,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /position_target Name: Marker @@ -209,7 +209,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /vehicle_pose_marker Name: Marker @@ -217,7 +217,7 @@ Visualization Manager: my_namespace: true Queue Size: 100 Value: true - - Class: rviz/InteractiveMarkers + - Class: rviz_default_plugins/InteractiveMarkers Enable Transparency: true Enabled: true Name: InteractiveMarkers @@ -226,7 +226,7 @@ Visualization Manager: Show Visual Aids: true Update Topic: /goal/update Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /arc_center Name: Marker @@ -234,7 +234,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /goal_marker Name: Marker @@ -244,7 +244,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 255; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 @@ -266,7 +266,7 @@ Visualization Manager: Topic: /reference/path Unreliable: false Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /viewpoints Name: MarkerArray @@ -274,7 +274,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: /tree Name: MarkerArray @@ -282,7 +282,7 @@ Visualization Manager: {} Queue Size: 100 Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /path_segments Name: MarkerArray @@ -290,7 +290,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /candidate_start_marker Name: Marker @@ -298,7 +298,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /candidate_goal_marker Name: Marker @@ -306,7 +306,7 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /vehicle_velocity Name: Marker @@ -338,7 +338,7 @@ Visualization Manager: Unreliable: false Use ColorMap: true Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /planned_viewpoints Name: MarkerArray @@ -354,26 +354,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 3920.333984375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/terrain_planner/launch/config_ompl.rviz b/terrain_planner/launch/config_ompl.rviz index 563a02b9..efd93928 100644 --- a/terrain_planner/launch/config_ompl.rviz +++ b/terrain_planner/launch/config_ompl.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -15,21 +15,21 @@ Panels: - /MarkerArray1 Splitter Ratio: 0.5 Tree Height: 746 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -43,7 +43,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -59,7 +59,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /start_position Name: Marker @@ -67,7 +67,7 @@ Visualization Manager: "": true Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /goal_position Name: Marker @@ -77,7 +77,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 @@ -100,7 +100,7 @@ Visualization Manager: Unreliable: false Value: true - Alpha: 1 - Class: rviz/Axes + Class: rviz_default_plugins/Axes Enabled: true Length: 1 Name: Axes @@ -180,7 +180,7 @@ Visualization Manager: Unreliable: false Use ColorMap: true Value: true - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /tree Name: MarkerArray @@ -197,26 +197,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 2900.530517578125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/terrain_planner/launch/config_ompl_segments.rviz b/terrain_planner/launch/config_ompl_segments.rviz index 05fe8386..31df2a0f 100644 --- a/terrain_planner/launch/config_ompl_segments.rviz +++ b/terrain_planner/launch/config_ompl_segments.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -16,21 +16,21 @@ Panels: - /Path2 Splitter Ratio: 0.5 Tree Height: 1106 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Name: Time SyncMode: 0 SyncSource: "" @@ -43,7 +43,7 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -59,7 +59,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /start_position Name: Marker @@ -67,7 +67,7 @@ Visualization Manager: "": true Queue Size: 100 Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true Marker Topic: /goal_position Name: Marker @@ -77,7 +77,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 @@ -100,7 +100,7 @@ Visualization Manager: Unreliable: false Value: false - Alpha: 1 - Class: rviz/Axes + Class: rviz_default_plugins/Axes Enabled: true Length: 1 Name: Axes @@ -180,7 +180,7 @@ Visualization Manager: Unreliable: false Use ColorMap: true Value: false - - Class: rviz/MarkerArray + - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: /path_segments Name: MarkerArray @@ -190,7 +190,7 @@ Visualization Manager: Value: true - Alpha: 1 Buffer Length: 1 - Class: rviz/Path + Class: rviz_default_plugins/Path Color: 252; 233; 79 Enabled: true Head Diameter: 0.30000001192092896 @@ -220,26 +220,26 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 1900.588623046875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 diff --git a/terrain_planner/launch/test_distance_surface.launch b/terrain_planner/launch/test_distance_surface.launch index fb11a196..8dad716a 100644 --- a/terrain_planner/launch/test_distance_surface.launch +++ b/terrain_planner/launch/test_distance_surface.launch @@ -7,6 +7,6 @@ - + diff --git a/terrain_planner/launch/test_ompl_dubins.launch b/terrain_planner/launch/test_ompl_dubins.launch index c4160b6f..915b3904 100644 --- a/terrain_planner/launch/test_ompl_dubins.launch +++ b/terrain_planner/launch/test_ompl_dubins.launch @@ -1,13 +1,12 @@ - + - + - - - + - - + + + diff --git a/terrain_planner/launch/test_ompl_dubins_to_circle.launch b/terrain_planner/launch/test_ompl_dubins_to_circle.launch index 3d667730..36629e03 100644 --- a/terrain_planner/launch/test_ompl_dubins_to_circle.launch +++ b/terrain_planner/launch/test_ompl_dubins_to_circle.launch @@ -8,6 +8,6 @@ - + diff --git a/terrain_planner/launch/test_ompl_rrt.launch b/terrain_planner/launch/test_ompl_rrt.launch index e812bf2d..6440e5d6 100644 --- a/terrain_planner/launch/test_ompl_rrt.launch +++ b/terrain_planner/launch/test_ompl_rrt.launch @@ -1,14 +1,17 @@ - + + + - + - - - + + + - - + + + diff --git a/terrain_planner/package.xml b/terrain_planner/package.xml index cfa2dff4..46d599d7 100644 --- a/terrain_planner/package.xml +++ b/terrain_planner/package.xml @@ -1,42 +1,48 @@ - + + terrain_planner 0.0.0 Terrain planner for safe navigation in complex terrain - Jaeyoung Lim - BSD - catkin - roscpp - rospy - std_msgs - sensor_msgs - nav_msgs - geometry_msgs + ament_cmake + + eigen grid_map_core grid_map_cv + grid_map_geo grid_map_ros + + ompl + rclcpp + rclpy + terrain_navigation + tf2 + tf2_eigen + yaml_cpp_vendor + + geometry_msgs grid_map_msgs - grid_map_pcl - grid_map_geo - tf + nav_msgs planner_msgs - terrain_navigation - ompl - roscpp - rospy - grid_map_visualization - grid_map_rviz_plugin - planner_msgs - terrain_navigation - grid_map_geo - ompl + sensor_msgs + std_msgs + tf2_geometry_msgs + + grid_map_geo + grid_map_rviz_plugin + grid_map_visualization + ompl + rclcpp + rclpy + terrain_navigation + yaml_cpp_vendor + planner_msgs - - + ament_cmake diff --git a/terrain_planner/src/DubinsAirplane.cpp b/terrain_planner/src/DubinsAirplane.cpp index bbedd4e2..8ed53962 100644 --- a/terrain_planner/src/DubinsAirplane.cpp +++ b/terrain_planner/src/DubinsAirplane.cpp @@ -852,7 +852,7 @@ std::tuple DubinsAirplaneStateSpace::addit double t_min = 0.0; double p_min = dp.getSegmentLength(3); double q_min = dp.getSegmentLength(4); - double x1_c, y1_c, z1_c, dx_c, dy_c, dz_c, d_c, th1_c, th_c, alpha_c, beta_c = 0.0; + double x1_c, y1_c, /*z1_c,*/ dx_c, dy_c, /*dz_c,*/ d_c, th1_c, th_c, alpha_c, beta_c = 0.0; ob::State* si = allocState(); DubinsPath dp_tmp; @@ -868,11 +868,11 @@ std::tuple DubinsAirplaneStateSpace::addit // extract state x1_c = si->as()->getX(); y1_c = si->as()->getY(); - z1_c = si->as()->getZ(); + // z1_c = si->as()->getZ(); th1_c = si->as()->getYaw(); dx_c = (x2 - x1_c) * curvature_; dy_c = (y2 - y1_c) * curvature_; - dz_c = (z2 - z1_c) * curvature_; + // dz_c = (z2 - z1_c) * curvature_; d_c = sqrtf(dx_c * dx_c + dy_c * dy_c); th_c = atan2f(dy_c, dx_c); alpha_c = mod2pi(th1_c - th_c); @@ -903,11 +903,11 @@ std::tuple DubinsAirplaneStateSpace::addit // extract state x1_c = si->as()->getX(); y1_c = si->as()->getY(); - z1_c = si->as()->getZ(); + // z1_c = si->as()->getZ(); th1_c = si->as()->getYaw(); dx_c = (x2 - x1_c) * curvature_; dy_c = (y2 - y1_c) * curvature_; - dz_c = (z2 - z1_c) * curvature_; + // dz_c = (z2 - z1_c) * curvature_; d_c = sqrtf(dx_c * dx_c + dy_c * dy_c); th_c = atan2f(dy_c, dx_c); alpha_c = mod2pi(th1_c - th_c); @@ -938,11 +938,11 @@ std::tuple DubinsAirplaneStateSpace::addit // extract state x1_c = si->as()->getX(); y1_c = si->as()->getY(); - z1_c = si->as()->getZ(); + // z1_c = si->as()->getZ(); th1_c = si->as()->getYaw(); dx_c = (x2 - x1_c) * curvature_; dy_c = (y2 - y1_c) * curvature_; - dz_c = (z2 - z1_c) * curvature_; + // dz_c = (z2 - z1_c) * curvature_; d_c = sqrtf(dx_c * dx_c + dy_c * dy_c); th_c = atan2f(dy_c, dx_c); alpha_c = mod2pi(th1_c - th_c); @@ -974,11 +974,11 @@ std::tuple DubinsAirplaneStateSpace::addit // extract state x1_c = si->as()->getX(); y1_c = si->as()->getY(); - z1_c = si->as()->getZ(); + // z1_c = si->as()->getZ(); th1_c = si->as()->getYaw(); dx_c = (x2 - x1_c) * curvature_; dy_c = (y2 - y1_c) * curvature_; - dz_c = (z2 - z1_c) * curvature_; + // dz_c = (z2 - z1_c) * curvature_; d_c = sqrtf(dx_c * dx_c + dy_c * dy_c); th_c = atan2f(dy_c, dx_c); alpha_c = mod2pi(th1_c - th_c); @@ -1071,7 +1071,7 @@ void DubinsAirplaneStateSpace::interpolate(const DubinsPath& path, const Segment else interpol_tanGamma_ = tanf(path.getGamma()); - for (interpol_iter_ = 0u; interpol_iter_ < 6u; ++interpol_iter_) { + for (interpol_iter_ = 0; interpol_iter_ < 6; ++interpol_iter_) { if ((interpol_seg_ < path.getSegmentLength(interpol_iter_) || (interpol_iter_ == 5u))) { stateInterpolation_->setXYZYaw(0.0, 0.0, 0.0, segmentStarts.segmentStarts[interpol_iter_].yaw); interpol_phiStart_ = stateInterpolation_->getYaw(); @@ -1141,7 +1141,7 @@ void DubinsAirplaneStateSpace::interpolate(const DubinsPath& path, const Segment return; } -void DubinsAirplaneStateSpace::interpolateWithWind(const ob::State* from, const DubinsPath& path, +void DubinsAirplaneStateSpace::interpolateWithWind(const ob::State* /*from*/, const DubinsPath& path, const SegmentStarts& segmentStarts, double t, ob::State* state) const { interpolate(path, segmentStarts, t, state); @@ -1160,7 +1160,7 @@ void DubinsAirplaneStateSpace::calculateSegmentStarts(const ob::State* from, con interpol_tanGamma_ = tanf(path.getGamma()); stateInterpolation_->setXYZYaw(0.0, 0.0, 0.0, from->as()->getYaw()); - for (interpol_iter_ = 0u; interpol_iter_ < 6u && interpol_seg_ > 0.0; ++interpol_iter_) { + for (interpol_iter_ = 0; interpol_iter_ < 6 && interpol_seg_ > 0.0; ++interpol_iter_) { interpol_v_ = std::min(interpol_seg_, path.getSegmentLength(interpol_iter_)); interpol_phiStart_ = stateInterpolation_->getYaw(); interpol_seg_ -= interpol_v_; @@ -1280,7 +1280,7 @@ unsigned int DubinsAirplaneStateSpace::convert_idx(unsigned int i) const { } } -double DubinsAirplaneStateSpace::t_lsr(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::t_lsr(double d, double alpha, double /*beta*/, double sa, double sb, double ca, double cb) const { const double tmp = -2.0 + d * d + 2.0 * (ca * cb + sa * sb + d * (sa + sb)); const double p = sqrtf(std::max(tmp, 0.0)); @@ -1288,13 +1288,13 @@ double DubinsAirplaneStateSpace::t_lsr(double d, double alpha, double beta, doub return mod2pi(-alpha + theta); // t } -double DubinsAirplaneStateSpace::p_lsr(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::p_lsr(double d, double /*alpha*/, double /*beta*/, double sa, double sb, double ca, double cb) const { const double tmp = -2.0 + d * d + 2.0 * (ca * cb + sa * sb + d * (sa + sb)); return sqrtf(std::max(tmp, 0.0)); // p } -double DubinsAirplaneStateSpace::q_lsr(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::q_lsr(double d, double /*alpha*/, double beta, double sa, double sb, double ca, double cb) const { const double tmp = -2.0 + d * d + 2.0 * (ca * cb + sa * sb + d * (sa + sb)); const double p = sqrtf(std::max(tmp, 0.0)); @@ -1302,7 +1302,7 @@ double DubinsAirplaneStateSpace::q_lsr(double d, double alpha, double beta, doub return mod2pi(-beta + theta); // q } -double DubinsAirplaneStateSpace::t_rsl(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::t_rsl(double d, double alpha, double /*beta*/, double sa, double sb, double ca, double cb) const { const double tmp = d * d - 2.0 + 2.0 * (ca * cb + sa * sb - d * (sa + sb)); const double p = sqrtf(std::max(tmp, 0.0)); @@ -1310,13 +1310,13 @@ double DubinsAirplaneStateSpace::t_rsl(double d, double alpha, double beta, doub return mod2pi(alpha - theta); // t } -double DubinsAirplaneStateSpace::p_rsl(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::p_rsl(double d, double /*alpha*/, double /*beta*/, double sa, double sb, double ca, double cb) const { const double tmp = d * d - 2.0 + 2.0 * (ca * cb + sa * sb - d * (sa + sb)); return sqrtf(std::max(tmp, 0.0)); // p } -double DubinsAirplaneStateSpace::q_rsl(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::q_rsl(double d, double /*alpha*/, double beta, double sa, double sb, double ca, double cb) const { const double tmp = d * d - 2.0 + 2.0 * (ca * cb + sa * sb - d * (sa + sb)); const double p = sqrtf(std::max(tmp, 0.)); @@ -1324,37 +1324,37 @@ double DubinsAirplaneStateSpace::q_rsl(double d, double alpha, double beta, doub return mod2pi(beta - theta); // q } -double DubinsAirplaneStateSpace::t_rsr(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::t_rsr(double d, double alpha, double /*beta*/, double sa, double sb, double ca, double cb) const { const double theta = atan2f(ca - cb, d - sa + sb); return mod2pi(alpha - theta); // t } -double DubinsAirplaneStateSpace::p_rsr(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::p_rsr(double d, double /*alpha*/, double /*beta*/, double sa, double sb, double ca, double cb) const { const double tmp = 2.0 + d * d - 2.0 * (ca * cb + sa * sb - d * (sb - sa)); return sqrtf(std::max(tmp, 0.0)); // p } -double DubinsAirplaneStateSpace::q_rsr(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::q_rsr(double d, double /*alpha*/, double beta, double sa, double sb, double ca, double cb) const { const double theta = atan2f(ca - cb, d - sa + sb); return mod2pi(-beta + theta); // q } -double DubinsAirplaneStateSpace::t_lsl(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::t_lsl(double d, double alpha, double /*beta*/, double sa, double sb, double ca, double cb) const { const double theta = atan2f(cb - ca, d + sa - sb); return mod2pi(-alpha + theta); // t } -double DubinsAirplaneStateSpace::p_lsl(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::p_lsl(double d, double /*alpha*/, double /*beta*/, double sa, double sb, double ca, double cb) const { const double tmp = 2.0 + d * d - 2.0 * (ca * cb + sa * sb - d * (sa - sb)); return sqrtf(std::max(tmp, 0.0)); // p } -double DubinsAirplaneStateSpace::q_lsl(double d, double alpha, double beta, double sa, double sb, double ca, +double DubinsAirplaneStateSpace::q_lsl(double d, double /*alpha*/, double beta, double sa, double sb, double ca, double cb) const { const double theta = atan2f(cb - ca, d + sa - sb); return mod2pi(beta - theta); // q diff --git a/terrain_planner/src/DubinsPath.cpp b/terrain_planner/src/DubinsPath.cpp index 6f51e40c..658534fa 100644 --- a/terrain_planner/src/DubinsPath.cpp +++ b/terrain_planner/src/DubinsPath.cpp @@ -27,19 +27,20 @@ const DubinsPath::DubinsPathSegmentType DubinsPath::dubinsPathType[6][3] = { {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT}, {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}}; DubinsPath::DubinsPath(Index type, double t, double p, double q, double gam, unsigned int ks, unsigned int ke, double r) - : k_start_(ks), - k_end_(ke), + : + length_{{0, t, 0, p, q, 0}}, + length_2D_(t + p + q), radiusRatio_{{r, r, r, r, r, r}}, radiusRatioInverse_{{1.0 / r, 1.0 / r, 1.0 / r, 1.0 / r, 1.0 / r, 1.0 / r}}, gamma_(gam), one_div_cos_abs_gamma_(1.0 / cosf(fabs(gamma_))), + k_end_(ke), + k_start_(ks), + classification_(NOT_ASSIGNED), + idx_(type), lmh_(DubinsPath::ALT_CASE_LOW), additionalManeuver_(false), - idx_(type), - foundOptimalPath_(true), - classification_(NOT_ASSIGNED), - length_{{0, t, 0, p, q, 0}}, - length_2D_(t + p + q) { + foundOptimalPath_(true) { // only 6 different types available assert(type < 6u); type_ = dubinsPathType[type]; diff --git a/terrain_planner/src/common.cpp b/terrain_planner/src/common.cpp new file mode 100644 index 00000000..8f58a8a8 --- /dev/null +++ b/terrain_planner/src/common.cpp @@ -0,0 +1,823 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Jaeyoung Lim. 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 terrain-navigation 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 OWNER 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. + * + ****************************************************************************/ + +#include "terrain_planner/common.h" + +// const std::map>> colorMap{ +// {"viridis", {{0.267004, 0.004874, 0.329415}, {0.268510, 0.009605, 0.335427}, {0.269944, 0.014625, 0.341379}, +// {0.271305, 0.019942, 0.347269}, {0.272594, 0.025563, 0.353093}, {0.273809, 0.031497, 0.358853}, +// {0.274952, 0.037752, 0.364543}, {0.276022, 0.044167, 0.370164}, {0.277018, 0.050344, 0.375715}, +// {0.277941, 0.056324, 0.381191}, {0.278791, 0.062145, 0.386592}, {0.279566, 0.067836, 0.391917}, +// {0.280267, 0.073417, 0.397163}, {0.280894, 0.078907, 0.402329}, {0.281446, 0.084320, 0.407414}, +// {0.281924, 0.089666, 0.412415}, {0.282327, 0.094955, 0.417331}, {0.282656, 0.100196, 0.422160}, +// {0.282910, 0.105393, 0.426902}, {0.283091, 0.110553, 0.431554}, {0.283197, 0.115680, 0.436115}, +// {0.283229, 0.120777, 0.440584}, {0.283187, 0.125848, 0.444960}, {0.283072, 0.130895, 0.449241}, +// {0.282884, 0.135920, 0.453427}, {0.282623, 0.140926, 0.457517}, {0.282290, 0.145912, 0.461510}, +// {0.281887, 0.150881, 0.465405}, {0.281412, 0.155834, 0.469201}, {0.280868, 0.160771, 0.472899}, +// {0.280255, 0.165693, 0.476498}, {0.279574, 0.170599, 0.479997}, {0.278826, 0.175490, 0.483397}, +// {0.278012, 0.180367, 0.486697}, {0.277134, 0.185228, 0.489898}, {0.276194, 0.190074, 0.493001}, +// {0.275191, 0.194905, 0.496005}, {0.274128, 0.199721, 0.498911}, {0.273006, 0.204520, 0.501721}, +// {0.271828, 0.209303, 0.504434}, {0.270595, 0.214069, 0.507052}, {0.269308, 0.218818, 0.509577}, +// {0.267968, 0.223549, 0.512008}, {0.266580, 0.228262, 0.514349}, {0.265145, 0.232956, 0.516599}, +// {0.263663, 0.237631, 0.518762}, {0.262138, 0.242286, 0.520837}, {0.260571, 0.246922, 0.522828}, +// {0.258965, 0.251537, 0.524736}, {0.257322, 0.256130, 0.526563}, {0.255645, 0.260703, 0.528312}, +// {0.253935, 0.265254, 0.529983}, {0.252194, 0.269783, 0.531579}, {0.250425, 0.274290, 0.533103}, +// {0.248629, 0.278775, 0.534556}, {0.246811, 0.283237, 0.535941}, {0.244972, 0.287675, 0.537260}, +// {0.243113, 0.292092, 0.538516}, {0.241237, 0.296485, 0.539709}, {0.239346, 0.300855, 0.540844}, +// {0.237441, 0.305202, 0.541921}, {0.235526, 0.309527, 0.542944}, {0.233603, 0.313828, 0.543914}, +// {0.231674, 0.318106, 0.544834}, {0.229739, 0.322361, 0.545706}, {0.227802, 0.326594, 0.546532}, +// {0.225863, 0.330805, 0.547314}, {0.223925, 0.334994, 0.548053}, {0.221989, 0.339161, 0.548752}, +// {0.220057, 0.343307, 0.549413}, {0.218130, 0.347432, 0.550038}, {0.216210, 0.351535, 0.550627}, +// {0.214298, 0.355619, 0.551184}, {0.212395, 0.359683, 0.551710}, {0.210503, 0.363727, 0.552206}, +// {0.208623, 0.367752, 0.552675}, {0.206756, 0.371758, 0.553117}, {0.204903, 0.375746, 0.553533}, +// {0.203063, 0.379716, 0.553925}, {0.201239, 0.383670, 0.554294}, {0.199430, 0.387607, 0.554642}, +// {0.197636, 0.391528, 0.554969}, {0.195860, 0.395433, 0.555276}, {0.194100, 0.399323, 0.555565}, +// {0.192357, 0.403199, 0.555836}, {0.190631, 0.407061, 0.556089}, {0.188923, 0.410910, 0.556326}, +// {0.187231, 0.414746, 0.556547}, {0.185556, 0.418570, 0.556753}, {0.183898, 0.422383, 0.556944}, +// {0.182256, 0.426184, 0.557120}, {0.180629, 0.429975, 0.557282}, {0.179019, 0.433756, 0.557430}, +// {0.177423, 0.437527, 0.557565}, {0.175841, 0.441290, 0.557685}, {0.174274, 0.445044, 0.557792}, +// {0.172719, 0.448791, 0.557885}, {0.171176, 0.452530, 0.557965}, {0.169646, 0.456262, 0.558030}, +// {0.168126, 0.459988, 0.558082}, {0.166617, 0.463708, 0.558119}, {0.165117, 0.467423, 0.558141}, +// {0.163625, 0.471133, 0.558148}, {0.162142, 0.474838, 0.558140}, {0.160665, 0.478540, 0.558115}, +// {0.159194, 0.482237, 0.558073}, {0.157729, 0.485932, 0.558013}, {0.156270, 0.489624, 0.557936}, +// {0.154815, 0.493313, 0.557840}, {0.153364, 0.497000, 0.557724}, {0.151918, 0.500685, 0.557587}, +// {0.150476, 0.504369, 0.557430}, {0.149039, 0.508051, 0.557250}, {0.147607, 0.511733, 0.557049}, +// {0.146180, 0.515413, 0.556823}, {0.144759, 0.519093, 0.556572}, {0.143343, 0.522773, 0.556295}, +// {0.141935, 0.526453, 0.555991}, {0.140536, 0.530132, 0.555659}, {0.139147, 0.533812, 0.555298}, +// {0.137770, 0.537492, 0.554906}, {0.136408, 0.541173, 0.554483}, {0.135066, 0.544853, 0.554029}, +// {0.133743, 0.548535, 0.553541}, {0.132444, 0.552216, 0.553018}, {0.131172, 0.555899, 0.552459}, +// {0.129933, 0.559582, 0.551864}, {0.128729, 0.563265, 0.551229}, {0.127568, 0.566949, 0.550556}, +// {0.126453, 0.570633, 0.549841}, {0.125394, 0.574318, 0.549086}, {0.124395, 0.578002, 0.548287}, +// {0.123463, 0.581687, 0.547445}, {0.122606, 0.585371, 0.546557}, {0.121831, 0.589055, 0.545623}, +// {0.121148, 0.592739, 0.544641}, {0.120565, 0.596422, 0.543611}, {0.120092, 0.600104, 0.542530}, +// {0.119738, 0.603785, 0.541400}, {0.119512, 0.607464, 0.540218}, {0.119423, 0.611141, 0.538982}, +// {0.119483, 0.614817, 0.537692}, {0.119699, 0.618490, 0.536347}, {0.120081, 0.622161, 0.534946}, +// {0.120638, 0.625828, 0.533488}, {0.121380, 0.629492, 0.531973}, {0.122312, 0.633153, 0.530398}, +// {0.123444, 0.636809, 0.528763}, {0.124780, 0.640461, 0.527068}, {0.126326, 0.644107, 0.525311}, +// {0.128087, 0.647749, 0.523491}, {0.130067, 0.651384, 0.521608}, {0.132268, 0.655014, 0.519661}, +// {0.134692, 0.658636, 0.517649}, {0.137339, 0.662252, 0.515571}, {0.140210, 0.665859, 0.513427}, +// {0.143303, 0.669459, 0.511215}, {0.146616, 0.673050, 0.508936}, {0.150148, 0.676631, 0.506589}, +// {0.153894, 0.680203, 0.504172}, {0.157851, 0.683765, 0.501686}, {0.162016, 0.687316, 0.499129}, +// {0.166383, 0.690856, 0.496502}, {0.170948, 0.694384, 0.493803}, {0.175707, 0.697900, 0.491033}, +// {0.180653, 0.701402, 0.488189}, {0.185783, 0.704891, 0.485273}, {0.191090, 0.708366, 0.482284}, +// {0.196571, 0.711827, 0.479221}, {0.202219, 0.715272, 0.476084}, {0.208030, 0.718701, 0.472873}, +// {0.214000, 0.722114, 0.469588}, {0.220124, 0.725509, 0.466226}, {0.226397, 0.728888, 0.462789}, +// {0.232815, 0.732247, 0.459277}, {0.239374, 0.735588, 0.455688}, {0.246070, 0.738910, 0.452024}, +// {0.252899, 0.742211, 0.448284}, {0.259857, 0.745492, 0.444467}, {0.266941, 0.748751, 0.440573}, +// {0.274149, 0.751988, 0.436601}, {0.281477, 0.755203, 0.432552}, {0.288921, 0.758394, 0.428426}, +// {0.296479, 0.761561, 0.424223}, {0.304148, 0.764704, 0.419943}, {0.311925, 0.767822, 0.415586}, +// {0.319809, 0.770914, 0.411152}, {0.327796, 0.773980, 0.406640}, {0.335885, 0.777018, 0.402049}, +// {0.344074, 0.780029, 0.397381}, {0.352360, 0.783011, 0.392636}, {0.360741, 0.785964, 0.387814}, +// {0.369214, 0.788888, 0.382914}, {0.377779, 0.791781, 0.377939}, {0.386433, 0.794644, 0.372886}, +// {0.395174, 0.797475, 0.367757}, {0.404001, 0.800275, 0.362552}, {0.412913, 0.803041, 0.357269}, +// {0.421908, 0.805774, 0.351910}, {0.430983, 0.808473, 0.346476}, {0.440137, 0.811138, 0.340967}, +// {0.449368, 0.813768, 0.335384}, {0.458674, 0.816363, 0.329727}, {0.468053, 0.818921, 0.323998}, +// {0.477504, 0.821444, 0.318195}, {0.487026, 0.823929, 0.312321}, {0.496615, 0.826376, 0.306377}, +// {0.506271, 0.828786, 0.300362}, {0.515992, 0.831158, 0.294279}, {0.525776, 0.833491, 0.288127}, +// {0.535621, 0.835785, 0.281908}, {0.545524, 0.838039, 0.275626}, {0.555484, 0.840254, 0.269281}, +// {0.565498, 0.842430, 0.262877}, {0.575563, 0.844566, 0.256415}, {0.585678, 0.846661, 0.249897}, +// {0.595839, 0.848717, 0.243329}, {0.606045, 0.850733, 0.236712}, {0.616293, 0.852709, 0.230052}, +// {0.626579, 0.854645, 0.223353}, {0.636902, 0.856542, 0.216620}, {0.647257, 0.858400, 0.209861}, +// {0.657642, 0.860219, 0.203082}, {0.668054, 0.861999, 0.196293}, {0.678489, 0.863742, 0.189503}, +// {0.688944, 0.865448, 0.182725}, {0.699415, 0.867117, 0.175971}, {0.709898, 0.868751, 0.169257}, +// {0.720391, 0.870350, 0.162603}, {0.730889, 0.871916, 0.156029}, {0.741388, 0.873449, 0.149561}, +// {0.751884, 0.874951, 0.143228}, {0.762373, 0.876424, 0.137064}, {0.772852, 0.877868, 0.131109}, +// {0.783315, 0.879285, 0.125405}, {0.793760, 0.880678, 0.120005}, {0.804182, 0.882046, 0.114965}, +// {0.814576, 0.883393, 0.110347}, {0.824940, 0.884720, 0.106217}, {0.835270, 0.886029, 0.102646}, +// {0.845561, 0.887322, 0.099702}, {0.855810, 0.888601, 0.097452}, {0.866013, 0.889868, 0.095953}, +// {0.876168, 0.891125, 0.095250}, {0.886271, 0.892374, 0.095374}, {0.896320, 0.893616, 0.096335}, +// {0.906311, 0.894855, 0.098125}, {0.916242, 0.896091, 0.100717}, {0.926106, 0.897330, 0.104071}, +// {0.935904, 0.898570, 0.108131}, {0.945636, 0.899815, 0.112838}, {0.955300, 0.901065, 0.118128}, +// {0.964894, 0.902323, 0.123941}, {0.974417, 0.903590, 0.130215}, {0.983868, 0.904867, 0.136897}, +// {0.993248, 0.906157, 0.143936}}}, +// {"magma", {{0.001462, 0.000466, 0.013866}, {0.002258, 0.001295, 0.018331}, {0.003279, 0.002305, 0.023708}, +// {0.004512, 0.003490, 0.029965}, {0.005950, 0.004843, 0.037130}, {0.007588, 0.006356, 0.044973}, +// {0.009426, 0.008022, 0.052844}, {0.011465, 0.009828, 0.060750}, {0.013708, 0.011771, 0.068667}, +// {0.016156, 0.013840, 0.076603}, {0.018815, 0.016026, 0.084584}, {0.021692, 0.018320, 0.092610}, +// {0.024792, 0.020715, 0.100676}, {0.028123, 0.023201, 0.108787}, {0.031696, 0.025765, 0.116965}, +// {0.035520, 0.028397, 0.125209}, {0.039608, 0.031090, 0.133515}, {0.043830, 0.033830, 0.141886}, +// {0.048062, 0.036607, 0.150327}, {0.052320, 0.039407, 0.158841}, {0.056615, 0.042160, 0.167446}, +// {0.060949, 0.044794, 0.176129}, {0.065330, 0.047318, 0.184892}, {0.069764, 0.049726, 0.193735}, +// {0.074257, 0.052017, 0.202660}, {0.078815, 0.054184, 0.211667}, {0.083446, 0.056225, 0.220755}, +// {0.088155, 0.058133, 0.229922}, {0.092949, 0.059904, 0.239164}, {0.097833, 0.061531, 0.248477}, +// {0.102815, 0.063010, 0.257854}, {0.107899, 0.064335, 0.267289}, {0.113094, 0.065492, 0.276784}, +// {0.118405, 0.066479, 0.286321}, {0.123833, 0.067295, 0.295879}, {0.129380, 0.067935, 0.305443}, +// {0.135053, 0.068391, 0.315000}, {0.140858, 0.068654, 0.324538}, {0.146785, 0.068738, 0.334011}, +// {0.152839, 0.068637, 0.343404}, {0.159018, 0.068354, 0.352688}, {0.165308, 0.067911, 0.361816}, +// {0.171713, 0.067305, 0.370771}, {0.178212, 0.066576, 0.379497}, {0.184801, 0.065732, 0.387973}, +// {0.191460, 0.064818, 0.396152}, {0.198177, 0.063862, 0.404009}, {0.204935, 0.062907, 0.411514}, +// {0.211718, 0.061992, 0.418647}, {0.218512, 0.061158, 0.425392}, {0.225302, 0.060445, 0.431742}, +// {0.232077, 0.059889, 0.437695}, {0.238826, 0.059517, 0.443256}, {0.245543, 0.059352, 0.448436}, +// {0.252220, 0.059415, 0.453248}, {0.258857, 0.059706, 0.457710}, {0.265447, 0.060237, 0.461840}, +// {0.271994, 0.060994, 0.465660}, {0.278493, 0.061978, 0.469190}, {0.284951, 0.063168, 0.472451}, +// {0.291366, 0.064553, 0.475462}, {0.297740, 0.066117, 0.478243}, {0.304081, 0.067835, 0.480812}, +// {0.310382, 0.069702, 0.483186}, {0.316654, 0.071690, 0.485380}, {0.322899, 0.073782, 0.487408}, +// {0.329114, 0.075972, 0.489287}, {0.335308, 0.078236, 0.491024}, {0.341482, 0.080564, 0.492631}, +// {0.347636, 0.082946, 0.494121}, {0.353773, 0.085373, 0.495501}, {0.359898, 0.087831, 0.496778}, +// {0.366012, 0.090314, 0.497960}, {0.372116, 0.092816, 0.499053}, {0.378211, 0.095332, 0.500067}, +// {0.384299, 0.097855, 0.501002}, {0.390384, 0.100379, 0.501864}, {0.396467, 0.102902, 0.502658}, +// {0.402548, 0.105420, 0.503386}, {0.408629, 0.107930, 0.504052}, {0.414709, 0.110431, 0.504662}, +// {0.420791, 0.112920, 0.505215}, {0.426877, 0.115395, 0.505714}, {0.432967, 0.117855, 0.506160}, +// {0.439062, 0.120298, 0.506555}, {0.445163, 0.122724, 0.506901}, {0.451271, 0.125132, 0.507198}, +// {0.457386, 0.127522, 0.507448}, {0.463508, 0.129893, 0.507652}, {0.469640, 0.132245, 0.507809}, +// {0.475780, 0.134577, 0.507921}, {0.481929, 0.136891, 0.507989}, {0.488088, 0.139186, 0.508011}, +// {0.494258, 0.141462, 0.507988}, {0.500438, 0.143719, 0.507920}, {0.506629, 0.145958, 0.507806}, +// {0.512831, 0.148179, 0.507648}, {0.519045, 0.150383, 0.507443}, {0.525270, 0.152569, 0.507192}, +// {0.531507, 0.154739, 0.506895}, {0.537755, 0.156894, 0.506551}, {0.544015, 0.159033, 0.506159}, +// {0.550287, 0.161158, 0.505719}, {0.556571, 0.163269, 0.505230}, {0.562866, 0.165368, 0.504692}, +// {0.569172, 0.167454, 0.504105}, {0.575490, 0.169530, 0.503466}, {0.581819, 0.171596, 0.502777}, +// {0.588158, 0.173652, 0.502035}, {0.594508, 0.175701, 0.501241}, {0.600868, 0.177743, 0.500394}, +// {0.607238, 0.179779, 0.499492}, {0.613617, 0.181811, 0.498536}, {0.620005, 0.183840, 0.497524}, +// {0.626401, 0.185867, 0.496456}, {0.632805, 0.187893, 0.495332}, {0.639216, 0.189921, 0.494150}, +// {0.645633, 0.191952, 0.492910}, {0.652056, 0.193986, 0.491611}, {0.658483, 0.196027, 0.490253}, +// {0.664915, 0.198075, 0.488836}, {0.671349, 0.200133, 0.487358}, {0.677786, 0.202203, 0.485819}, +// {0.684224, 0.204286, 0.484219}, {0.690661, 0.206384, 0.482558}, {0.697098, 0.208501, 0.480835}, +// {0.703532, 0.210638, 0.479049}, {0.709962, 0.212797, 0.477201}, {0.716387, 0.214982, 0.475290}, +// {0.722805, 0.217194, 0.473316}, {0.729216, 0.219437, 0.471279}, {0.735616, 0.221713, 0.469180}, +// {0.742004, 0.224025, 0.467018}, {0.748378, 0.226377, 0.464794}, {0.754737, 0.228772, 0.462509}, +// {0.761077, 0.231214, 0.460162}, {0.767398, 0.233705, 0.457755}, {0.773695, 0.236249, 0.455289}, +// {0.779968, 0.238851, 0.452765}, {0.786212, 0.241514, 0.450184}, {0.792427, 0.244242, 0.447543}, +// {0.798608, 0.247040, 0.444848}, {0.804752, 0.249911, 0.442102}, {0.810855, 0.252861, 0.439305}, +// {0.816914, 0.255895, 0.436461}, {0.822926, 0.259016, 0.433573}, {0.828886, 0.262229, 0.430644}, +// {0.834791, 0.265540, 0.427671}, {0.840636, 0.268953, 0.424666}, {0.846416, 0.272473, 0.421631}, +// {0.852126, 0.276106, 0.418573}, {0.857763, 0.279857, 0.415496}, {0.863320, 0.283729, 0.412403}, +// {0.868793, 0.287728, 0.409303}, {0.874176, 0.291859, 0.406205}, {0.879464, 0.296125, 0.403118}, +// {0.884651, 0.300530, 0.400047}, {0.889731, 0.305079, 0.397002}, {0.894700, 0.309773, 0.393995}, +// {0.899552, 0.314616, 0.391037}, {0.904281, 0.319610, 0.388137}, {0.908884, 0.324755, 0.385308}, +// {0.913354, 0.330052, 0.382563}, {0.917689, 0.335500, 0.379915}, {0.921884, 0.341098, 0.377376}, +// {0.925937, 0.346844, 0.374959}, {0.929845, 0.352734, 0.372677}, {0.933606, 0.358764, 0.370541}, +// {0.937221, 0.364929, 0.368567}, {0.940687, 0.371224, 0.366762}, {0.944006, 0.377643, 0.365136}, +// {0.947180, 0.384178, 0.363701}, {0.950210, 0.390820, 0.362468}, {0.953099, 0.397563, 0.361438}, +// {0.955849, 0.404400, 0.360619}, {0.958464, 0.411324, 0.360014}, {0.960949, 0.418323, 0.359630}, +// {0.963310, 0.425390, 0.359469}, {0.965549, 0.432519, 0.359529}, {0.967671, 0.439703, 0.359810}, +// {0.969680, 0.446936, 0.360311}, {0.971582, 0.454210, 0.361030}, {0.973381, 0.461520, 0.361965}, +// {0.975082, 0.468861, 0.363111}, {0.976690, 0.476226, 0.364466}, {0.978210, 0.483612, 0.366025}, +// {0.979645, 0.491014, 0.367783}, {0.981000, 0.498428, 0.369734}, {0.982279, 0.505851, 0.371874}, +// {0.983485, 0.513280, 0.374198}, {0.984622, 0.520713, 0.376698}, {0.985693, 0.528148, 0.379371}, +// {0.986700, 0.535582, 0.382210}, {0.987646, 0.543015, 0.385210}, {0.988533, 0.550446, 0.388365}, +// {0.989363, 0.557873, 0.391671}, {0.990138, 0.565296, 0.395122}, {0.990871, 0.572706, 0.398714}, +// {0.991558, 0.580107, 0.402441}, {0.992196, 0.587502, 0.406299}, {0.992785, 0.594891, 0.410283}, +// {0.993326, 0.602275, 0.414390}, {0.993834, 0.609644, 0.418613}, {0.994309, 0.616999, 0.422950}, +// {0.994738, 0.624350, 0.427397}, {0.995122, 0.631696, 0.431951}, {0.995480, 0.639027, 0.436607}, +// {0.995810, 0.646344, 0.441361}, {0.996096, 0.653659, 0.446213}, {0.996341, 0.660969, 0.451160}, +// {0.996580, 0.668256, 0.456192}, {0.996775, 0.675541, 0.461314}, {0.996925, 0.682828, 0.466526}, +// {0.997077, 0.690088, 0.471811}, {0.997186, 0.697349, 0.477182}, {0.997254, 0.704611, 0.482635}, +// {0.997325, 0.711848, 0.488154}, {0.997351, 0.719089, 0.493755}, {0.997351, 0.726324, 0.499428}, +// {0.997341, 0.733545, 0.505167}, {0.997285, 0.740772, 0.510983}, {0.997228, 0.747981, 0.516859}, +// {0.997138, 0.755190, 0.522806}, {0.997019, 0.762398, 0.528821}, {0.996898, 0.769591, 0.534892}, +// {0.996727, 0.776795, 0.541039}, {0.996571, 0.783977, 0.547233}, {0.996369, 0.791167, 0.553499}, +// {0.996162, 0.798348, 0.559820}, {0.995932, 0.805527, 0.566202}, {0.995680, 0.812706, 0.572645}, +// {0.995424, 0.819875, 0.579140}, {0.995131, 0.827052, 0.585701}, {0.994851, 0.834213, 0.592307}, +// {0.994524, 0.841387, 0.598983}, {0.994222, 0.848540, 0.605696}, {0.993866, 0.855711, 0.612482}, +// {0.993545, 0.862859, 0.619299}, {0.993170, 0.870024, 0.626189}, {0.992831, 0.877168, 0.633109}, +// {0.992440, 0.884330, 0.640099}, {0.992089, 0.891470, 0.647116}, {0.991688, 0.898627, 0.654202}, +// {0.991332, 0.905763, 0.661309}, {0.990930, 0.912915, 0.668481}, {0.990570, 0.920049, 0.675675}, +// {0.990175, 0.927196, 0.682926}, {0.989815, 0.934329, 0.690198}, {0.989434, 0.941470, 0.697519}, +// {0.989077, 0.948604, 0.704863}, {0.988717, 0.955742, 0.712242}, {0.988367, 0.962878, 0.719649}, +// {0.988033, 0.970012, 0.727077}, {0.987691, 0.977154, 0.734536}, {0.987387, 0.984288, 0.742002}, +// {0.987053, 0.991438, 0.749504}}}, +// {"plasma", {{0.050383, 0.029803, 0.527975}, {0.063536, 0.028426, 0.533124}, {0.075353, 0.027206, 0.538007}, +// {0.086222, 0.026125, 0.542658}, {0.096379, 0.025165, 0.547103}, {0.105980, 0.024309, 0.551368}, +// {0.115124, 0.023556, 0.555468}, {0.123903, 0.022878, 0.559423}, {0.132381, 0.022258, 0.563250}, +// {0.140603, 0.021687, 0.566959}, {0.148607, 0.021154, 0.570562}, {0.156421, 0.020651, 0.574065}, +// {0.164070, 0.020171, 0.577478}, {0.171574, 0.019706, 0.580806}, {0.178950, 0.019252, 0.584054}, +// {0.186213, 0.018803, 0.587228}, {0.193374, 0.018354, 0.590330}, {0.200445, 0.017902, 0.593364}, +// {0.207435, 0.017442, 0.596333}, {0.214350, 0.016973, 0.599239}, {0.221197, 0.016497, 0.602083}, +// {0.227983, 0.016007, 0.604867}, {0.234715, 0.015502, 0.607592}, {0.241396, 0.014979, 0.610259}, +// {0.248032, 0.014439, 0.612868}, {0.254627, 0.013882, 0.615419}, {0.261183, 0.013308, 0.617911}, +// {0.267703, 0.012716, 0.620346}, {0.274191, 0.012109, 0.622722}, {0.280648, 0.011488, 0.625038}, +// {0.287076, 0.010855, 0.627295}, {0.293478, 0.010213, 0.629490}, {0.299855, 0.009561, 0.631624}, +// {0.306210, 0.008902, 0.633694}, {0.312543, 0.008239, 0.635700}, {0.318856, 0.007576, 0.637640}, +// {0.325150, 0.006915, 0.639512}, {0.331426, 0.006261, 0.641316}, {0.337683, 0.005618, 0.643049}, +// {0.343925, 0.004991, 0.644710}, {0.350150, 0.004382, 0.646298}, {0.356359, 0.003798, 0.647810}, +// {0.362553, 0.003243, 0.649245}, {0.368733, 0.002724, 0.650601}, {0.374897, 0.002245, 0.651876}, +// {0.381047, 0.001814, 0.653068}, {0.387183, 0.001434, 0.654177}, {0.393304, 0.001114, 0.655199}, +// {0.399411, 0.000859, 0.656133}, {0.405503, 0.000678, 0.656977}, {0.411580, 0.000577, 0.657730}, +// {0.417642, 0.000564, 0.658390}, {0.423689, 0.000646, 0.658956}, {0.429719, 0.000831, 0.659425}, +// {0.435734, 0.001127, 0.659797}, {0.441732, 0.001540, 0.660069}, {0.447714, 0.002080, 0.660240}, +// {0.453677, 0.002755, 0.660310}, {0.459623, 0.003574, 0.660277}, {0.465550, 0.004545, 0.660139}, +// {0.471457, 0.005678, 0.659897}, {0.477344, 0.006980, 0.659549}, {0.483210, 0.008460, 0.659095}, +// {0.489055, 0.010127, 0.658534}, {0.494877, 0.011990, 0.657865}, {0.500678, 0.014055, 0.657088}, +// {0.506454, 0.016333, 0.656202}, {0.512206, 0.018833, 0.655209}, {0.517933, 0.021563, 0.654109}, +// {0.523633, 0.024532, 0.652901}, {0.529306, 0.027747, 0.651586}, {0.534952, 0.031217, 0.650165}, +// {0.540570, 0.034950, 0.648640}, {0.546157, 0.038954, 0.647010}, {0.551715, 0.043136, 0.645277}, +// {0.557243, 0.047331, 0.643443}, {0.562738, 0.051545, 0.641509}, {0.568201, 0.055778, 0.639477}, +// {0.573632, 0.060028, 0.637349}, {0.579029, 0.064296, 0.635126}, {0.584391, 0.068579, 0.632812}, +// {0.589719, 0.072878, 0.630408}, {0.595011, 0.077190, 0.627917}, {0.600266, 0.081516, 0.625342}, +// {0.605485, 0.085854, 0.622686}, {0.610667, 0.090204, 0.619951}, {0.615812, 0.094564, 0.617140}, +// {0.620919, 0.098934, 0.614257}, {0.625987, 0.103312, 0.611305}, {0.631017, 0.107699, 0.608287}, +// {0.636008, 0.112092, 0.605205}, {0.640959, 0.116492, 0.602065}, {0.645872, 0.120898, 0.598867}, +// {0.650746, 0.125309, 0.595617}, {0.655580, 0.129725, 0.592317}, {0.660374, 0.134144, 0.588971}, +// {0.665129, 0.138566, 0.585582}, {0.669845, 0.142992, 0.582154}, {0.674522, 0.147419, 0.578688}, +// {0.679160, 0.151848, 0.575189}, {0.683758, 0.156278, 0.571660}, {0.688318, 0.160709, 0.568103}, +// {0.692840, 0.165141, 0.564522}, {0.697324, 0.169573, 0.560919}, {0.701769, 0.174005, 0.557296}, +// {0.706178, 0.178437, 0.553657}, {0.710549, 0.182868, 0.550004}, {0.714883, 0.187299, 0.546338}, +// {0.719181, 0.191729, 0.542663}, {0.723444, 0.196158, 0.538981}, {0.727670, 0.200586, 0.535293}, +// {0.731862, 0.205013, 0.531601}, {0.736019, 0.209439, 0.527908}, {0.740143, 0.213864, 0.524216}, +// {0.744232, 0.218288, 0.520524}, {0.748289, 0.222711, 0.516834}, {0.752312, 0.227133, 0.513149}, +// {0.756304, 0.231555, 0.509468}, {0.760264, 0.235976, 0.505794}, {0.764193, 0.240396, 0.502126}, +// {0.768090, 0.244817, 0.498465}, {0.771958, 0.249237, 0.494813}, {0.775796, 0.253658, 0.491171}, +// {0.779604, 0.258078, 0.487539}, {0.783383, 0.262500, 0.483918}, {0.787133, 0.266922, 0.480307}, +// {0.790855, 0.271345, 0.476706}, {0.794549, 0.275770, 0.473117}, {0.798216, 0.280197, 0.469538}, +// {0.801855, 0.284626, 0.465971}, {0.805467, 0.289057, 0.462415}, {0.809052, 0.293491, 0.458870}, +// {0.812612, 0.297928, 0.455338}, {0.816144, 0.302368, 0.451816}, {0.819651, 0.306812, 0.448306}, +// {0.823132, 0.311261, 0.444806}, {0.826588, 0.315714, 0.441316}, {0.830018, 0.320172, 0.437836}, +// {0.833422, 0.324635, 0.434366}, {0.836801, 0.329105, 0.430905}, {0.840155, 0.333580, 0.427455}, +// {0.843484, 0.338062, 0.424013}, {0.846788, 0.342551, 0.420579}, {0.850066, 0.347048, 0.417153}, +// {0.853319, 0.351553, 0.413734}, {0.856547, 0.356066, 0.410322}, {0.859750, 0.360588, 0.406917}, +// {0.862927, 0.365119, 0.403519}, {0.866078, 0.369660, 0.400126}, {0.869203, 0.374212, 0.396738}, +// {0.872303, 0.378774, 0.393355}, {0.875376, 0.383347, 0.389976}, {0.878423, 0.387932, 0.386600}, +// {0.881443, 0.392529, 0.383229}, {0.884436, 0.397139, 0.379860}, {0.887402, 0.401762, 0.376494}, +// {0.890340, 0.406398, 0.373130}, {0.893250, 0.411048, 0.369768}, {0.896131, 0.415712, 0.366407}, +// {0.898984, 0.420392, 0.363047}, {0.901807, 0.425087, 0.359688}, {0.904601, 0.429797, 0.356329}, +// {0.907365, 0.434524, 0.352970}, {0.910098, 0.439268, 0.349610}, {0.912800, 0.444029, 0.346251}, +// {0.915471, 0.448807, 0.342890}, {0.918109, 0.453603, 0.339529}, {0.920714, 0.458417, 0.336166}, +// {0.923287, 0.463251, 0.332801}, {0.925825, 0.468103, 0.329435}, {0.928329, 0.472975, 0.326067}, +// {0.930798, 0.477867, 0.322697}, {0.933232, 0.482780, 0.319325}, {0.935630, 0.487712, 0.315952}, +// {0.937990, 0.492667, 0.312575}, {0.940313, 0.497642, 0.309197}, {0.942598, 0.502639, 0.305816}, +// {0.944844, 0.507658, 0.302433}, {0.947051, 0.512699, 0.299049}, {0.949217, 0.517763, 0.295662}, +// {0.951344, 0.522850, 0.292275}, {0.953428, 0.527960, 0.288883}, {0.955470, 0.533093, 0.285490}, +// {0.957469, 0.538250, 0.282096}, {0.959424, 0.543431, 0.278701}, {0.961336, 0.548636, 0.275305}, +// {0.963203, 0.553865, 0.271909}, {0.965024, 0.559118, 0.268513}, {0.966798, 0.564396, 0.265118}, +// {0.968526, 0.569700, 0.261721}, {0.970205, 0.575028, 0.258325}, {0.971835, 0.580382, 0.254931}, +// {0.973416, 0.585761, 0.251540}, {0.974947, 0.591165, 0.248151}, {0.976428, 0.596595, 0.244767}, +// {0.977856, 0.602051, 0.241387}, {0.979233, 0.607532, 0.238013}, {0.980556, 0.613039, 0.234646}, +// {0.981826, 0.618572, 0.231287}, {0.983041, 0.624131, 0.227937}, {0.984199, 0.629718, 0.224595}, +// {0.985301, 0.635330, 0.221265}, {0.986345, 0.640969, 0.217948}, {0.987332, 0.646633, 0.214648}, +// {0.988260, 0.652325, 0.211364}, {0.989128, 0.658043, 0.208100}, {0.989935, 0.663787, 0.204859}, +// {0.990681, 0.669558, 0.201642}, {0.991365, 0.675355, 0.198453}, {0.991985, 0.681179, 0.195295}, +// {0.992541, 0.687030, 0.192170}, {0.993032, 0.692907, 0.189084}, {0.993456, 0.698810, 0.186041}, +// {0.993814, 0.704741, 0.183043}, {0.994103, 0.710698, 0.180097}, {0.994324, 0.716681, 0.177208}, +// {0.994474, 0.722691, 0.174381}, {0.994553, 0.728728, 0.171622}, {0.994561, 0.734791, 0.168938}, +// {0.994495, 0.740880, 0.166335}, {0.994355, 0.746995, 0.163821}, {0.994141, 0.753137, 0.161404}, +// {0.993851, 0.759304, 0.159092}, {0.993482, 0.765499, 0.156891}, {0.993033, 0.771720, 0.154808}, +// {0.992505, 0.777967, 0.152855}, {0.991897, 0.784239, 0.151042}, {0.991209, 0.790537, 0.149377}, +// {0.990439, 0.796859, 0.147870}, {0.989587, 0.803205, 0.146529}, {0.988648, 0.809579, 0.145357}, +// {0.987621, 0.815978, 0.144363}, {0.986509, 0.822401, 0.143557}, {0.985314, 0.828846, 0.142945}, +// {0.984031, 0.835315, 0.142528}, {0.982653, 0.841812, 0.142303}, {0.981190, 0.848329, 0.142279}, +// {0.979644, 0.854866, 0.142453}, {0.977995, 0.861432, 0.142808}, {0.976265, 0.868016, 0.143351}, +// {0.974443, 0.874622, 0.144061}, {0.972530, 0.881250, 0.144923}, {0.970533, 0.887896, 0.145919}, +// {0.968443, 0.894564, 0.147014}, {0.966271, 0.901249, 0.148180}, {0.964021, 0.907950, 0.149370}, +// {0.961681, 0.914672, 0.150520}, {0.959276, 0.921407, 0.151566}, {0.956808, 0.928152, 0.152409}, +// {0.954287, 0.934908, 0.152921}, {0.951726, 0.941671, 0.152925}, {0.949151, 0.948435, 0.152178}, +// {0.946602, 0.955190, 0.150328}, {0.944152, 0.961916, 0.146861}, {0.941896, 0.968590, 0.140956}, +// {0.940015, 0.975158, 0.131326}}}, +// {"gist_rainbow", {{1.000000, 0.000000, 0.160000}, {1.000000, 0.000000, 0.139085}, {1.000000, 0.000000, 0.118170}, +// {1.000000, 0.000000, 0.097255}, {1.000000, 0.000000, 0.076340}, {1.000000, 0.000000, 0.055425}, +// {1.000000, 0.000000, 0.034510}, {1.000000, 0.000000, 0.013595}, {1.000000, 0.007419, 0.000000}, +// {1.000000, 0.028617, 0.000000}, {1.000000, 0.049815, 0.000000}, {1.000000, 0.071012, 0.000000}, +// {1.000000, 0.092210, 0.000000}, {1.000000, 0.113408, 0.000000}, {1.000000, 0.134605, 0.000000}, +// {1.000000, 0.155803, 0.000000}, {1.000000, 0.177001, 0.000000}, {1.000000, 0.198198, 0.000000}, +// {1.000000, 0.219396, 0.000000}, {1.000000, 0.240594, 0.000000}, {1.000000, 0.261791, 0.000000}, +// {1.000000, 0.282989, 0.000000}, {1.000000, 0.304187, 0.000000}, {1.000000, 0.325384, 0.000000}, +// {1.000000, 0.346582, 0.000000}, {1.000000, 0.367780, 0.000000}, {1.000000, 0.388977, 0.000000}, +// {1.000000, 0.410175, 0.000000}, {1.000000, 0.431373, 0.000000}, {1.000000, 0.452570, 0.000000}, +// {1.000000, 0.473768, 0.000000}, {1.000000, 0.494966, 0.000000}, {1.000000, 0.516163, 0.000000}, +// {1.000000, 0.537361, 0.000000}, {1.000000, 0.558559, 0.000000}, {1.000000, 0.579756, 0.000000}, +// {1.000000, 0.600954, 0.000000}, {1.000000, 0.622152, 0.000000}, {1.000000, 0.643349, 0.000000}, +// {1.000000, 0.664547, 0.000000}, {1.000000, 0.685745, 0.000000}, {1.000000, 0.706942, 0.000000}, +// {1.000000, 0.728140, 0.000000}, {1.000000, 0.749338, 0.000000}, {1.000000, 0.770535, 0.000000}, +// {1.000000, 0.791733, 0.000000}, {1.000000, 0.812931, 0.000000}, {1.000000, 0.834128, 0.000000}, +// {1.000000, 0.855326, 0.000000}, {1.000000, 0.876524, 0.000000}, {1.000000, 0.897721, 0.000000}, +// {1.000000, 0.918919, 0.000000}, {1.000000, 0.940117, 0.000000}, {1.000000, 0.961314, 0.000000}, +// {1.000000, 0.982512, 0.000000}, {0.996290, 1.000000, 0.000000}, {0.975093, 1.000000, 0.000000}, +// {0.953895, 1.000000, 0.000000}, {0.932697, 1.000000, 0.000000}, {0.911500, 1.000000, 0.000000}, +// {0.890302, 1.000000, 0.000000}, {0.869104, 1.000000, 0.000000}, {0.847907, 1.000000, 0.000000}, +// {0.826709, 1.000000, 0.000000}, {0.805511, 1.000000, 0.000000}, {0.784314, 1.000000, 0.000000}, +// {0.763116, 1.000000, 0.000000}, {0.741918, 1.000000, 0.000000}, {0.720721, 1.000000, 0.000000}, +// {0.699523, 1.000000, 0.000000}, {0.678325, 1.000000, 0.000000}, {0.657128, 1.000000, 0.000000}, +// {0.635930, 1.000000, 0.000000}, {0.614732, 1.000000, 0.000000}, {0.593535, 1.000000, 0.000000}, +// {0.572337, 1.000000, 0.000000}, {0.551139, 1.000000, 0.000000}, {0.529942, 1.000000, 0.000000}, +// {0.508744, 1.000000, 0.000000}, {0.487546, 1.000000, 0.000000}, {0.466349, 1.000000, 0.000000}, +// {0.445151, 1.000000, 0.000000}, {0.423953, 1.000000, 0.000000}, {0.402756, 1.000000, 0.000000}, +// {0.381558, 1.000000, 0.000000}, {0.360360, 1.000000, 0.000000}, {0.339163, 1.000000, 0.000000}, +// {0.317965, 1.000000, 0.000000}, {0.296767, 1.000000, 0.000000}, {0.275570, 1.000000, 0.000000}, +// {0.254372, 1.000000, 0.000000}, {0.233174, 1.000000, 0.000000}, {0.211977, 1.000000, 0.000000}, +// {0.190779, 1.000000, 0.000000}, {0.169581, 1.000000, 0.000000}, {0.148384, 1.000000, 0.000000}, +// {0.127186, 1.000000, 0.000000}, {0.105988, 1.000000, 0.000000}, {0.084791, 1.000000, 0.000000}, +// {0.063593, 1.000000, 0.000000}, {0.042395, 1.000000, 0.000000}, {0.021198, 1.000000, 0.000000}, +// {0.000000, 1.000000, 0.000000}, {0.000000, 1.000000, 0.021084}, {0.000000, 1.000000, 0.042167}, +// {0.000000, 1.000000, 0.063251}, {0.000000, 1.000000, 0.084335}, {0.000000, 1.000000, 0.105419}, +// {0.000000, 1.000000, 0.126502}, {0.000000, 1.000000, 0.147586}, {0.000000, 1.000000, 0.168670}, +// {0.000000, 1.000000, 0.189753}, {0.000000, 1.000000, 0.210837}, {0.000000, 1.000000, 0.231921}, +// {0.000000, 1.000000, 0.253004}, {0.000000, 1.000000, 0.274088}, {0.000000, 1.000000, 0.295172}, +// {0.000000, 1.000000, 0.316256}, {0.000000, 1.000000, 0.337339}, {0.000000, 1.000000, 0.358423}, +// {0.000000, 1.000000, 0.379507}, {0.000000, 1.000000, 0.400590}, {0.000000, 1.000000, 0.421674}, +// {0.000000, 1.000000, 0.442758}, {0.000000, 1.000000, 0.463841}, {0.000000, 1.000000, 0.484925}, +// {0.000000, 1.000000, 0.506009}, {0.000000, 1.000000, 0.527093}, {0.000000, 1.000000, 0.548176}, +// {0.000000, 1.000000, 0.569260}, {0.000000, 1.000000, 0.590344}, {0.000000, 1.000000, 0.611427}, +// {0.000000, 1.000000, 0.632511}, {0.000000, 1.000000, 0.653595}, {0.000000, 1.000000, 0.674678}, +// {0.000000, 1.000000, 0.695762}, {0.000000, 1.000000, 0.716846}, {0.000000, 1.000000, 0.737930}, +// {0.000000, 1.000000, 0.759013}, {0.000000, 1.000000, 0.780097}, {0.000000, 1.000000, 0.801181}, +// {0.000000, 1.000000, 0.822264}, {0.000000, 1.000000, 0.843348}, {0.000000, 1.000000, 0.864432}, +// {0.000000, 1.000000, 0.885515}, {0.000000, 1.000000, 0.906599}, {0.000000, 1.000000, 0.927683}, +// {0.000000, 1.000000, 0.948767}, {0.000000, 1.000000, 0.969850}, {0.000000, 1.000000, 0.990934}, +// {0.000000, 0.987852, 1.000000}, {0.000000, 0.966539, 1.000000}, {0.000000, 0.945226, 1.000000}, +// {0.000000, 0.923913, 1.000000}, {0.000000, 0.902600, 1.000000}, {0.000000, 0.881287, 1.000000}, +// {0.000000, 0.859974, 1.000000}, {0.000000, 0.838662, 1.000000}, {0.000000, 0.817349, 1.000000}, +// {0.000000, 0.796036, 1.000000}, {0.000000, 0.774723, 1.000000}, {0.000000, 0.753410, 1.000000}, +// {0.000000, 0.732097, 1.000000}, {0.000000, 0.710784, 1.000000}, {0.000000, 0.689471, 1.000000}, +// {0.000000, 0.668159, 1.000000}, {0.000000, 0.646846, 1.000000}, {0.000000, 0.625533, 1.000000}, +// {0.000000, 0.604220, 1.000000}, {0.000000, 0.582907, 1.000000}, {0.000000, 0.561594, 1.000000}, +// {0.000000, 0.540281, 1.000000}, {0.000000, 0.518968, 1.000000}, {0.000000, 0.497656, 1.000000}, +// {0.000000, 0.476343, 1.000000}, {0.000000, 0.455030, 1.000000}, {0.000000, 0.433717, 1.000000}, +// {0.000000, 0.412404, 1.000000}, {0.000000, 0.391091, 1.000000}, {0.000000, 0.369778, 1.000000}, +// {0.000000, 0.348465, 1.000000}, {0.000000, 0.327153, 1.000000}, {0.000000, 0.305840, 1.000000}, +// {0.000000, 0.284527, 1.000000}, {0.000000, 0.263214, 1.000000}, {0.000000, 0.241901, 1.000000}, +// {0.000000, 0.220588, 1.000000}, {0.000000, 0.199275, 1.000000}, {0.000000, 0.177962, 1.000000}, +// {0.000000, 0.156650, 1.000000}, {0.000000, 0.135337, 1.000000}, {0.000000, 0.114024, 1.000000}, +// {0.000000, 0.092711, 1.000000}, {0.000000, 0.071398, 1.000000}, {0.000000, 0.050085, 1.000000}, +// {0.000000, 0.028772, 1.000000}, {0.000000, 0.007460, 1.000000}, {0.013853, 0.000000, 1.000000}, +// {0.035166, 0.000000, 1.000000}, {0.056479, 0.000000, 1.000000}, {0.077792, 0.000000, 1.000000}, +// {0.099105, 0.000000, 1.000000}, {0.120418, 0.000000, 1.000000}, {0.141731, 0.000000, 1.000000}, +// {0.163043, 0.000000, 1.000000}, {0.184356, 0.000000, 1.000000}, {0.205669, 0.000000, 1.000000}, +// {0.226982, 0.000000, 1.000000}, {0.248295, 0.000000, 1.000000}, {0.269608, 0.000000, 1.000000}, +// {0.290921, 0.000000, 1.000000}, {0.312234, 0.000000, 1.000000}, {0.333546, 0.000000, 1.000000}, +// {0.354859, 0.000000, 1.000000}, {0.376172, 0.000000, 1.000000}, {0.397485, 0.000000, 1.000000}, +// {0.418798, 0.000000, 1.000000}, {0.440111, 0.000000, 1.000000}, {0.461424, 0.000000, 1.000000}, +// {0.482737, 0.000000, 1.000000}, {0.504049, 0.000000, 1.000000}, {0.525362, 0.000000, 1.000000}, +// {0.546675, 0.000000, 1.000000}, {0.567988, 0.000000, 1.000000}, {0.589301, 0.000000, 1.000000}, +// {0.610614, 0.000000, 1.000000}, {0.631927, 0.000000, 1.000000}, {0.653240, 0.000000, 1.000000}, +// {0.674552, 0.000000, 1.000000}, {0.695865, 0.000000, 1.000000}, {0.717178, 0.000000, 1.000000}, +// {0.738491, 0.000000, 1.000000}, {0.759804, 0.000000, 1.000000}, {0.781117, 0.000000, 1.000000}, +// {0.802430, 0.000000, 1.000000}, {0.823743, 0.000000, 1.000000}, {0.845055, 0.000000, 1.000000}, +// {0.866368, 0.000000, 1.000000}, {0.887681, 0.000000, 1.000000}, {0.908994, 0.000000, 1.000000}, +// {0.930307, 0.000000, 1.000000}, {0.951620, 0.000000, 1.000000}, {0.972933, 0.000000, 1.000000}, +// {0.994246, 0.000000, 1.000000}, {1.000000, 0.000000, 0.984442}, {1.000000, 0.000000, 0.963129}, +// {1.000000, 0.000000, 0.941816}, {1.000000, 0.000000, 0.920503}, {1.000000, 0.000000, 0.899190}, +// {1.000000, 0.000000, 0.877877}, {1.000000, 0.000000, 0.856564}, {1.000000, 0.000000, 0.835251}, +// {1.000000, 0.000000, 0.813939}, {1.000000, 0.000000, 0.792626}, {1.000000, 0.000000, 0.771313}, +// {1.000000, 0.000000, 0.750000}}}}; + +Eigen::Vector3d toEigen(const geometry_msgs::msg::Point &p) { + Eigen::Vector3d ev3(p.x, p.y, p.z); + return ev3; +} + +Eigen::Vector3d toEigen(const geometry_msgs::msg::Vector3 &v3) { + Eigen::Vector3d ev3(v3.x, v3.y, v3.z); + return ev3; +} + +Eigen::Vector3d toEigen(const geometry_msgs::msg::Pose &p) { + Eigen::Vector3d position(p.position.x, p.position.y, p.position.z); + return position; +} + +geometry_msgs::msg::Vector3 toVector3(const Eigen::Vector3d &p) { + geometry_msgs::msg::Vector3 vector; + vector.x = p(0); + vector.y = p(1); + vector.z = p(2); + return vector; +} + +geometry_msgs::msg::Pose vector3d2PoseMsg(const Eigen::Vector3d position, + const Eigen::Vector4d orientation) { + geometry_msgs::msg::Pose encode_msg; + + encode_msg.orientation.w = orientation(0); + encode_msg.orientation.x = orientation(1); + encode_msg.orientation.y = orientation(2); + encode_msg.orientation.z = orientation(3); + encode_msg.position.x = position(0); + encode_msg.position.y = position(1); + encode_msg.position.z = position(2); + return encode_msg; +} + +geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg( + const Eigen::Vector3d position, const Eigen::Vector4d orientation) { + geometry_msgs::msg::PoseStamped encode_msg; + + encode_msg.header.stamp = rclcpp::Clock().now(); + encode_msg.header.frame_id = "map"; + encode_msg.pose.orientation.w = orientation(0); + encode_msg.pose.orientation.x = orientation(1); + encode_msg.pose.orientation.y = orientation(2); + encode_msg.pose.orientation.z = orientation(3); + encode_msg.pose.position.x = position(0); + encode_msg.pose.position.y = position(1); + encode_msg.pose.position.z = position(2); + return encode_msg; +} + +visualization_msgs::msg::Marker utility2MarkerMsg(const double utility, + const Eigen::Vector3d position, int id) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "my_namespace"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = position(0); + marker.pose.position.y = position(1); + marker.pose.position.z = position(2); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + double vis_utility; + if (utility < 1.0) { + vis_utility = 1.0 / 1; + } else { + vis_utility = utility / 1; + } + marker.scale.x = vis_utility; + marker.scale.y = vis_utility; + marker.scale.z = vis_utility; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + return marker; +} + +visualization_msgs::msg::Marker vector2ArrowsMsg( + const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, + Eigen::Vector3d color, const std::string marker_namespace) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = marker_namespace; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + geometry_msgs::msg::Point head; + head.x = position(0); + head.y = position(1); + head.z = position(2); + points.push_back(head); + geometry_msgs::msg::Point tail; + tail.x = position(0) + normal(0); + tail.y = position(1) + normal(1); + tail.z = position(2) + normal(2); + points.push_back(tail); + + marker.points = points; + marker.scale.x = std::min(normal.norm(), 1.0); + marker.scale.y = std::min(normal.norm(), 2.0); + marker.scale.z = 0.0; + marker.color.a = 1.0; + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + return marker; +} + +visualization_msgs::msg::Marker trajectory2MarkerMsg(Path &trajectory, + const int id) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "normals"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + for (auto position : trajectory.position()) { + geometry_msgs::msg::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + } + marker.points = points; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.5; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.a = 0.2; + if (!trajectory.valid()) { + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + } else { + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + } + return marker; +} + +visualization_msgs::msg::Marker trajectory2MarkerMsg( + std::vector &trajectory, const int id, + std::string color_map) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "normals"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + double max_altitude = -std::numeric_limits::infinity(); + double min_altitude = std::numeric_limits::infinity(); + for (auto &position : trajectory) { + geometry_msgs::msg::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + if (position(2) < min_altitude) { + min_altitude = position(2); + } + if (position(2) > max_altitude) { + max_altitude = position(2); + } + } + marker.points = points; + + std::vector colors; + for (auto &position : trajectory) { + double intensity = (position(2) - min_altitude) / (max_altitude - min_altitude); + intensity = std::min(intensity, 1.0); + intensity = std::max(intensity, 0.0); + + const std::vector> &ctable = colorMap.at(color_map); + + int idx = int(floor(intensity * 255)); + idx = std::min(idx, 255); + idx = std::max(idx, 0); + + // Get color from table + std::vector rgb = ctable.at(idx); + + std_msgs::msg::ColorRGBA color; + color.r = rgb[0]; + color.g = rgb[1]; + color.b = rgb[2]; + color.a = 0.8; + colors.push_back(color); + } + marker.colors = colors; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 2.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + return marker; +} + +visualization_msgs::msg::Marker trajectory2MarkerMsg( + std::vector &trajectory, const int id, + std::vector &trajectory_color) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "normals"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + double max_altitude = -std::numeric_limits::infinity(); + double min_altitude = std::numeric_limits::infinity(); + for (auto &position : trajectory) { + geometry_msgs::msg::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + if (position(2) < min_altitude) { + min_altitude = position(2); + } + if (position(2) > max_altitude) { + max_altitude = position(2); + } + } + marker.points = points; + + std::vector colors; + for (auto &code : trajectory_color) { + std_msgs::msg::ColorRGBA color; + color.r = code[0]; + color.g = code[1]; + color.b = code[2]; + color.a = 0.8; + colors.push_back(color); + } + marker.colors = colors; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 2.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + return marker; +} + +visualization_msgs::msg::Marker trajectory2MarkerMsg( + std::vector &trajectory, const int id, + Eigen::Vector3d color) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "normals"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + for (auto &position : trajectory) { + geometry_msgs::msg::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + } + marker.points = points; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 2.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + marker.color.a = 0.8; + marker.color.r = color.x(); + marker.color.g = color.y(); + marker.color.b = color.z(); + + return marker; +} + +visualization_msgs::msg::Marker trajectory2MarkerMsg( + PathSegment &trajectory, const int id, + Eigen::Vector3d color) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "normals"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + for (auto &position : trajectory.position()) { + geometry_msgs::msg::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + } + marker.points = points; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 2.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + marker.color.a = 1.0; + marker.color.r = color.x(); + marker.color.g = color.y(); + marker.color.b = color.z(); + + return marker; +} + +visualization_msgs::msg::Marker point2MarkerMsg(Eigen::Vector3d &position, + const int id, Eigen::Vector3d color) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "normals"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + geometry_msgs::msg::Point point; + marker.pose.position.x = position(0); + marker.pose.position.y = position(1); + marker.pose.position.z = position(2); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 2.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; + marker.color.r = color.x(); + marker.color.g = color.y(); + marker.color.b = color.z(); + + return marker; +} + +double GetTimeInSeconds(std::string date_time) { + std::stringstream ss(date_time); + std::string tagged_time; + std::string tagged_date; + + ss >> tagged_date >> tagged_time; + + std::stringstream ss_time(tagged_time); + std::vector time_hour; + + while (ss_time.good()) { + std::string substr; + std::getline(ss_time, substr, ':'); + time_hour.push_back(substr); + } + + return 3600.0 * std::stof(time_hour[0]) + 60.0 * std::stof(time_hour[1]) + std::stoi(time_hour[2]); +} + +double StringToGeoReference(std::string &exif_tag) { + std::stringstream ss(exif_tag); + std::vector result; + while (ss.good()) { + std::string substr; + std::getline(ss, substr, '('); + std::getline(ss, substr, ')'); + result.push_back(substr); + } + double output; + if (result.size() >= 4) { + /// TODO: Check precision and coordinate frame of this conversion + output = std::stod(result[0]) + 0.0166667 * std::stod(result[1]) + 0.000277778 * std::stod(result[2]); + } else { + output = std::stod(result[0]); + } + return output; +} + +bool parseAttitudeFromText(std::string text_path, std::string image_file, + Eigen::Vector4d &attitude) { + std::ifstream file(text_path); + std::string str; + + // Look for the image file name in the path + while (getline(file, str)) { + if (str.find(image_file) != std::string::npos) { + std::stringstream ss(str); + std::vector camera_pose; + camera_pose.resize(9); + // # IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME + ss >> camera_pose[0] >> camera_pose[1] >> camera_pose[2] >> camera_pose[3] >> camera_pose[4]; + attitude << std::stof(camera_pose[1]), std::stof(camera_pose[2]), std::stof(camera_pose[3]), + std::stof(camera_pose[4]); + return true; + } + } + return false; +} + +double getRandom(double min, double max) { + return std::abs(max - min) * static_cast(rand()) / static_cast(RAND_MAX) + std::min(max, min); +} + +Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, + const Eigen::Vector4d &p) { + Eigen::Vector4d quat; + quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2), + p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0); + return quat; +} + +bool validatePosition(const grid_map::GridMap &map, const Eigen::Vector3d goal, + Eigen::Vector3d &valid_goal) { + double upper_surface = map.atPosition("ics_+", goal.head(2)); + double lower_surface = map.atPosition("ics_-", goal.head(2)); + const bool is_goal_valid = (upper_surface < lower_surface) ? true : false; + valid_goal(0) = goal(0); + valid_goal(1) = goal(1); + valid_goal(2) = (upper_surface + lower_surface) / 2.0; + return is_goal_valid; +} diff --git a/terrain_planner/src/test_ompl_dubins.cpp b/terrain_planner/src/test_ompl_dubins.cpp index 8bc39e31..95582942 100644 --- a/terrain_planner/src/test_ompl_dubins.cpp +++ b/terrain_planner/src/test_ompl_dubins.cpp @@ -39,20 +39,28 @@ * @author Jaeyoung Lim */ -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include + +#include +#include +#include #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" #include "terrain_planner/visualization.h" -void getDubinsShortestPath(const Eigen::Vector3d start_pos, const double start_yaw, const Eigen::Vector3d goal_pos, - const double goal_yaw, std::vector& path) { +using namespace std::chrono_literals; + +void getDubinsShortestPath( + const Eigen::Vector3d start_pos, + const double start_yaw, + const Eigen::Vector3d goal_pos, + const double goal_yaw, + std::vector& path) { auto dubins_ss = std::make_shared(); ompl::base::State* from = dubins_ss->allocState(); @@ -78,53 +86,77 @@ void getDubinsShortestPath(const Eigen::Vector3d start_pos, const double start_y if (interpolated_state(0) >= std::numeric_limits::max() && publish) { std::cout << "interpolated state had nans!" << std::endl; std::cout << " - start_yaw: " << start_yaw << " goal_yaw: " << goal_yaw << std::endl; - const double curvature = dubins_ss->getCurvature(); - const double dx = (goal_pos(0) - start_pos(0)) * curvature; - const double dy = (goal_pos(1) - start_pos(1)) * curvature; - const double dz = (goal_pos(2) - start_pos(2)) * curvature; - const double fabs_dz = fabs(dz); - const double th = atan2f(dy, dx); - const double alpha = start_yaw - th; - const double beta = goal_yaw - th; + //! @todo(srmainwaring) unused - prob intend for display. + // const double curvature = dubins_ss->getCurvature(); + // const double dx = (goal_pos(0) - start_pos(0)) * curvature; + // const double dy = (goal_pos(1) - start_pos(1)) * curvature; + // const double dz = (goal_pos(2) - start_pos(2)) * curvature; + // const double fabs_dz = fabs(dz); + // const double th = atan2f(dy, dx); + // const double alpha = start_yaw - th; + // const double beta = goal_yaw - th; publish = false; } path.push_back(interpolated_state); } } +class OmplRrtPlanner : public rclcpp::Node +{ + public: + OmplRrtPlanner() + : Node("ompl_rrt_planner") + { + // Initialize ROS related publishers for visualization + start_pos_pub = this->create_publisher("start_position", 1); + goal_pos_pub = this->create_publisher("goal_position", 1); + path_pub = this->create_publisher("path", 1); + trajectory_pub = this->create_publisher("tree", 1); + + std::vector start_yaw{0.0, 2.51681, 2.71681, 3.71681, 3.91681}; + std::vector goal_yaw{3.53454, 6.17454, 6.23454, 0.25135, 0.31135}; + + timer = this->create_wall_timer( + 1s, std::bind(&OmplRrtPlanner::timer_callback, this)); + } + + void timer_callback() { + Eigen::Vector3d start_pos(0.0, 0.0, 0.0); + Eigen::Vector3d goal_pos(152.15508, 0.0, 0.0); + + std::vector path; + + getDubinsShortestPath(start_pos, start_yaw[idx % start_yaw.size()], + goal_pos, goal_yaw[idx % goal_yaw.size()], + path); + + publishTrajectory(path_pub, path); + Eigen::Vector3d start_velocity(std::cos(start_yaw[idx % start_yaw.size()]), + std::sin(start_yaw[idx % start_yaw.size()]), + 0.0); + publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); + Eigen::Vector3d goal_velocity(std::cos(goal_yaw[idx % goal_yaw.size()]), + std::sin(goal_yaw[idx % goal_yaw.size()]), + 0.0); + publishPositionSetpoints(goal_pos_pub, goal_pos, goal_velocity); + idx++; + } + + private: + std::vector start_yaw{0.0, 2.51681, 2.71681, 3.71681, 3.91681}; + std::vector goal_yaw{3.53454, 6.17454, 6.23454, 0.25135, 0.31135}; + int idx{0}; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr start_pos_pub; + rclcpp::Publisher::SharedPtr goal_pos_pub; + rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr trajectory_pub; +}; + int main(int argc, char** argv) { - ros::init(argc, argv, "ompl_rrt_planner"); - ros::NodeHandle nh(""); - ros::NodeHandle nh_private("~"); - - // Initialize ROS related publishers for visualization - auto start_pos_pub = nh.advertise("start_position", 1, true); - auto goal_pos_pub = nh.advertise("goal_position", 1, true); - auto path_pub = nh.advertise("path", 1, true); - auto trajectory_pub = nh.advertise("tree", 1, true); - - std::vector start_yaw{0.0, 2.51681, 2.71681, 3.71681, 3.91681}; - std::vector goal_yaw{3.53454, 6.17454, 6.23454, 0.25135, 0.31135}; - int idx = 0; - while (true) { - Eigen::Vector3d start_pos(0.0, 0.0, 0.0); - Eigen::Vector3d goal_pos(152.15508, 0.0, 0.0); - - std::vector path; - - getDubinsShortestPath(start_pos, start_yaw[idx % start_yaw.size()], goal_pos, goal_yaw[idx % goal_yaw.size()], - path); - - publishTrajectory(path_pub, path); - Eigen::Vector3d start_velocity(std::cos(start_yaw[idx % start_yaw.size()]), - std::sin(start_yaw[idx % start_yaw.size()]), 0.0); - publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); - Eigen::Vector3d goal_velocity(std::cos(goal_yaw[idx % goal_yaw.size()]), std::sin(goal_yaw[idx % goal_yaw.size()]), - 0.0); - publishPositionSetpoints(goal_pos_pub, goal_pos, goal_velocity); - ros::Duration(1.0).sleep(); - idx++; - } - ros::spin(); + rclcpp::init(argc, argv); + auto ompl_rrt_planner = std::make_shared(); + rclcpp::spin(ompl_rrt_planner); + rclcpp::shutdown(); return 0; } diff --git a/terrain_planner/src/test_ompl_dubins_to_circle.cpp b/terrain_planner/src/test_ompl_dubins_to_circle.cpp index ad1cd2d2..2dc35ab7 100644 --- a/terrain_planner/src/test_ompl_dubins_to_circle.cpp +++ b/terrain_planner/src/test_ompl_dubins_to_circle.cpp @@ -38,37 +38,44 @@ * @author Jaeyoung Lim */ -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include + +#include +#include +#include #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" #include "terrain_planner/visualization.h" +using namespace std::chrono_literals; + double mod2pi(double x) { return x - 2 * M_PI * floor(x * (0.5 / M_PI)); } -void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, const double radius) { - visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; +void publishCircleSetpoints( + rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d& position, + const double radius) { + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Clock().now(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; marker.header.frame_id = "map"; marker.id = 0; - marker.header.stamp = ros::Time::now(); - std::vector points; + marker.header.stamp = rclcpp::Clock().now(); + std::vector points; for (double t = 0.0; t <= 1.0; t += 0.02) { - geometry_msgs::Point point; + geometry_msgs::msg::Point point; point.x = position.x() + radius * std::cos(t * 2 * M_PI); point.y = position.y() + radius * std::sin(t * 2 * M_PI); point.z = position.z(); points.push_back(point); } - geometry_msgs::Point start_point; + geometry_msgs::msg::Point start_point; start_point.x = position.x() + radius * std::cos(0.0); start_point.y = position.y() + radius * std::sin(0.0); start_point.z = position.z(); @@ -86,7 +93,7 @@ void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& po marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; - pub.publish(marker); + pub->publish(marker); } void getDubinsShortestPath(std::shared_ptr& dubins_ss, @@ -170,51 +177,71 @@ double getDubinsTangentPoint(std::shared_ptrcreate_publisher("start_position", 1); + goal_pos_pub = this->create_publisher("goal_position", 1); + tangent_pos_pub = this->create_publisher("tangent_position", 1); + path_pub = this->create_publisher("path", 1); + path_pub_2 = this->create_publisher("candidate_path", 1); + trajectory_pub = this->create_publisher("tree", 1); + + timer = this->create_wall_timer( + 1s, std::bind(&OmplRrtPlanner::timer_callback, this)); + } + + void timer_callback() { + auto dubins_ss = std::make_shared(); + Eigen::Vector3d start_pos(0.0, 0.0, 0.0); + /// Goal circular radius + Eigen::Vector3d goal_pos(400.0, 0.0, 0.0); + double goal_radius = 66.6667; + double tangent_yaw = getDubinsTangentPoint(dubins_ss, start_pos, start_yaw, goal_pos, goal_radius); + + /// TODO: Compare two different positions with same tangent yaw + Eigen::Vector3d tangent_pos; + tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw - 0.5 * M_PI), + goal_pos.y() + goal_radius * std::sin(tangent_yaw - 0.5 * M_PI), goal_pos.z(); + std::vector path_candidate_1; + getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_1); + + tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw + 0.5 * M_PI), + goal_pos.y() + goal_radius * std::sin(tangent_yaw + 0.5 * M_PI), goal_pos.z(); + std::vector path_candidate_2; + getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_2); + + // Visualize + publishTrajectory(path_pub, path_candidate_1); + publishTrajectory(path_pub_2, path_candidate_2); + Eigen::Vector3d start_velocity(std::cos(start_yaw), std::sin(start_yaw), 0.0); + publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); + Eigen::Vector3d tangent_velocity(std::cos(tangent_yaw), std::sin(tangent_yaw), 0.0); + publishPositionSetpoints(tangent_pos_pub, tangent_pos, tangent_velocity); + publishCircleSetpoints(goal_pos_pub, goal_pos, goal_radius); + start_yaw += 0.1; + } + + private: + double start_yaw{M_PI_2}; + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr start_pos_pub; + rclcpp::Publisher::SharedPtr goal_pos_pub; + rclcpp::Publisher::SharedPtr tangent_pos_pub; + rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr path_pub_2; + rclcpp::Publisher::SharedPtr trajectory_pub; +}; + int main(int argc, char** argv) { - ros::init(argc, argv, "ompl_rrt_planner"); - ros::NodeHandle nh(""); - ros::NodeHandle nh_private("~"); - - // Initialize ROS related publishers for visualization - auto start_pos_pub = nh.advertise("start_position", 1, true); - auto goal_pos_pub = nh.advertise("goal_position", 1, true); - auto tangent_pos_pub = nh.advertise("tangent_position", 1, true); - auto path_pub = nh.advertise("path", 1, true); - auto path_pub_2 = nh.advertise("candidate_path", 1, true); - auto trajectory_pub = nh.advertise("tree", 1, true); - - double start_yaw{M_PI_2}; - while (true) { - auto dubins_ss = std::make_shared(); - Eigen::Vector3d start_pos(0.0, 0.0, 0.0); - /// Goal circular radius - Eigen::Vector3d goal_pos(400.0, 0.0, 0.0); - double goal_radius = 66.6667; - double tangent_yaw = getDubinsTangentPoint(dubins_ss, start_pos, start_yaw, goal_pos, goal_radius); - - /// TODO: Compare two different positions with same tangent yaw - Eigen::Vector3d tangent_pos; - tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw - 0.5 * M_PI), - goal_pos.y() + goal_radius * std::sin(tangent_yaw - 0.5 * M_PI), goal_pos.z(); - std::vector path_candidate_1; - getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_1); - - tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw + 0.5 * M_PI), - goal_pos.y() + goal_radius * std::sin(tangent_yaw + 0.5 * M_PI), goal_pos.z(); - std::vector path_candidate_2; - getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_2); - - // Visualize - publishTrajectory(path_pub, path_candidate_1); - publishTrajectory(path_pub_2, path_candidate_2); - Eigen::Vector3d start_velocity(std::cos(start_yaw), std::sin(start_yaw), 0.0); - publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); - Eigen::Vector3d tangent_velocity(std::cos(tangent_yaw), std::sin(tangent_yaw), 0.0); - publishPositionSetpoints(tangent_pos_pub, tangent_pos, tangent_velocity); - publishCircleSetpoints(goal_pos_pub, goal_pos, goal_radius); - start_yaw += 0.1; - ros::Duration(1.0).sleep(); - } - ros::spin(); + rclcpp::init(argc, argv); + auto ompl_rrt_planner = std::make_shared(); + rclcpp::spin(ompl_rrt_planner); + rclcpp::shutdown(); return 0; } diff --git a/terrain_planner/src/test_rrt_node.cpp b/terrain_planner/src/test_rrt_node.cpp index 511d4adc..f6b51965 100644 --- a/terrain_planner/src/test_rrt_node.cpp +++ b/terrain_planner/src/test_rrt_node.cpp @@ -39,30 +39,51 @@ * @author Jaeyoung Lim */ -#include -#include +#include +#include +#include +#include + +#include +#include #include #include -#include -#include -#include -#include +#include + +#include +#include +#include #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" #include "terrain_planner/visualization.h" -void publishPathSegments(ros::Publisher& pub, Path& trajectory) { - visualization_msgs::MarkerArray msg; +//! @todo(srmainwaring)check includes +// #include "terrain_planner/common.h" +// #include "terrain_planner/DubinsAirplane.hpp" +// #include "terrain_planner/DubinsPath.hpp" +// #include "terrain_planner/maneuver_library.h" +// #include "terrain_planner/ompl_setup.h" +// #include "terrain_planner/planner.h" +// #include "terrain_planner/terrain_ompl_rrt.h" +// #include "terrain_planner/terrain_ompl.h" +#include "terrain_planner/visualization.h" + +using namespace std::chrono_literals; + +void publishPathSegments( + rclcpp::Publisher::SharedPtr pub, + Path& trajectory) { + visualization_msgs::msg::MarkerArray msg; - std::vector marker; - visualization_msgs::Marker mark; - mark.action = visualization_msgs::Marker::DELETEALL; + std::vector marker; + visualization_msgs::msg::Marker mark; + mark.action = visualization_msgs::msg::Marker::DELETEALL; marker.push_back(mark); msg.markers = marker; - pub.publish(msg); + pub->publish(msg); - std::vector segment_markers; + std::vector segment_markers; int i = 0; for (auto& segment : trajectory.segments) { Eigen::Vector3d color = Eigen::Vector3d(1.0, 0.0, 0.0); @@ -77,87 +98,123 @@ void publishPathSegments(ros::Publisher& pub, Path& trajectory) { segment_markers.insert(segment_markers.begin(), point2MarkerMsg(segment.position().back(), i++, color)); } msg.markers = segment_markers; - pub.publish(msg); + pub->publish(msg); } -int main(int argc, char** argv) { - ros::init(argc, argv, "ompl_rrt_planner"); - ros::NodeHandle nh(""); - ros::NodeHandle nh_private("~"); - - // Initialize ROS related publishers for visualization - auto start_pos_pub = nh.advertise("start_position", 1, true); - auto goal_pos_pub = nh.advertise("goal_position", 1, true); - auto path_pub = nh.advertise("path", 1, true); - auto interpolate_path_pub = nh.advertise("interpolated_path", 1, true); - auto path_segment_pub = nh.advertise("path_segments", 1, true); - auto grid_map_pub = nh.advertise("grid_map", 1, true); - auto trajectory_pub = nh.advertise("tree", 1, true); - std::string map_path, color_file_path; - bool random{false}; - nh_private.param("map_path", map_path, ""); - nh_private.param("color_file_path", color_file_path, ""); - nh_private.param("random", random, false); - - // Load terrain map from defined tif paths - auto terrain_map = std::make_shared(); - terrain_map->initializeFromGeotiff(map_path, false); - if (!color_file_path.empty()) { // Load color layer if the color path is nonempty - terrain_map->addColorFromGeotiff(color_file_path); - } - terrain_map->AddLayerDistanceTransform(50.0, "distance_surface"); - terrain_map->AddLayerDistanceTransform(120.0, "max_elevation"); - - Path path; - std::vector interpolated_path; - double terrain_altitude{100.0}; - - int num_experiments{20}; - for (int i = 0; i < num_experiments; i++) { - // Initialize planner with loaded terrain map - auto planner = std::make_shared(); - planner->setMap(terrain_map); - planner->setAltitudeLimits(120.0, 50.0); - /// TODO: Get bounds from gridmap - planner->setBoundsFromMap(terrain_map->getGridMap()); - - const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition(); - const double map_width_x = terrain_map->getGridMap().getLength().x(); - const double map_width_y = terrain_map->getGridMap().getLength().y(); - - Eigen::Vector3d start{Eigen::Vector3d(map_pos(0) - 0.4 * map_width_x, map_pos(1) - 0.4 * map_width_y, 0.0)}; - start(2) = - terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(start(0), start(1))) + terrain_altitude; - double start_yaw = getRandom(-M_PI, M_PI); - Eigen::Vector3d start_vel = 10.0 * Eigen::Vector3d(std::cos(start_yaw), std::sin(start_yaw), 0.0); - Eigen::Vector3d goal{Eigen::Vector3d(map_pos(0) + 0.4 * map_width_x, map_pos(1) + 0.4 * map_width_y, 0.0)}; - goal(2) = terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(goal(0), goal(1))) + terrain_altitude; - double goal_yaw = getRandom(-M_PI, M_PI); - Eigen::Vector3d goal_vel = 10.0 * Eigen::Vector3d(std::cos(goal_yaw), std::sin(goal_yaw), 0.0); - - planner->setupProblem(start, start_vel, goal, goal_vel); - bool found_solution{false}; - while (!found_solution) { - found_solution = planner->Solve(1.0, path); +class OmplRrtPlanner : public rclcpp::Node +{ + public: + OmplRrtPlanner() + : Node("ompl_rrt_planner") + { + // Initialize ROS related publishers for visualization + start_pos_pub = this->create_publisher("start_position", 1); + goal_pos_pub = this->create_publisher("goal_position", 1); + path_pub = this->create_publisher("path", 1); + interpolate_path_pub = this->create_publisher("interpolated_path", 1); + path_segment_pub = this->create_publisher("path_segments", 1); + grid_map_pub = this->create_publisher("grid_map", 1); + trajectory_pub = this->create_publisher("tree", 1); + + map_path = this->declare_parameter("map_path", "."); + color_file_path = this->declare_parameter("color_file_path", "."); + random = this->declare_parameter("random", random); + + RCLCPP_INFO_STREAM(get_logger(), "map_path: " << map_path); + RCLCPP_INFO_STREAM(get_logger(), "color_file_path: " << color_file_path); + RCLCPP_INFO_STREAM(get_logger(), "random: " << random); + + // Load terrain map from defined tif paths + terrain_map = std::make_shared(); + terrain_map->initializeFromGeotiff(map_path, false); + // Load color layer if the color path is nonempty + if (!color_file_path.empty()) { + terrain_map->addColorFromGeotiff(color_file_path); + } + terrain_map->AddLayerDistanceTransform(50.0, "distance_surface"); + terrain_map->AddLayerDistanceTransform(120.0, "max_elevation"); + + timer = this->create_wall_timer( + 1s, std::bind(&OmplRrtPlanner::timer_callback, this)); } - planner->getSolutionPath(interpolated_path); - - // Repeatedly publish results - terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec()); - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap(), message); - grid_map_pub.publish(message); - publishTrajectory(path_pub, path.position()); - publishTrajectory(interpolate_path_pub, interpolated_path); - publishPathSegments(path_segment_pub, path); - publishPositionSetpoints(start_pos_pub, start, start_vel); - publishPositionSetpoints(goal_pos_pub, goal, goal_vel); - publishTree(trajectory_pub, planner->getPlannerData(), planner->getProblemSetup()); - if (!random) { - break; + + void timer_callback() { + // for (int i = 0; i < num_experiments; i++) { + if (count_experiments++ < num_experiments) { + // Initialize planner with loaded terrain map + auto planner = std::make_shared(); + planner->setMap(terrain_map); + planner->setAltitudeLimits(120.0, 50.0); + /// TODO: Get bounds from gridmap + planner->setBoundsFromMap(terrain_map->getGridMap()); + + const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition(); + const double map_width_x = terrain_map->getGridMap().getLength().x(); + const double map_width_y = terrain_map->getGridMap().getLength().y(); + + Eigen::Vector3d start{Eigen::Vector3d(map_pos(0) - 0.4 * map_width_x, map_pos(1) - 0.4 * map_width_y, 0.0)}; + start(2) = + terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(start(0), start(1))) + terrain_altitude; + double start_yaw = getRandom(-M_PI, M_PI); + Eigen::Vector3d start_vel = 10.0 * Eigen::Vector3d(std::cos(start_yaw), std::sin(start_yaw), 0.0); + Eigen::Vector3d goal{Eigen::Vector3d(map_pos(0) + 0.4 * map_width_x, map_pos(1) + 0.4 * map_width_y, 0.0)}; + goal(2) = terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(goal(0), goal(1))) + terrain_altitude; + double goal_yaw = getRandom(-M_PI, M_PI); + Eigen::Vector3d goal_vel = 10.0 * Eigen::Vector3d(std::cos(goal_yaw), std::sin(goal_yaw), 0.0); + + planner->setupProblem(start, start_vel, goal, goal_vel); + bool found_solution{false}; + while (!found_solution) { + found_solution = planner->Solve(1.0, path); + } + planner->getSolutionPath(interpolated_path); + + // Repeatedly publish results + terrain_map->getGridMap().setTimestamp(this->get_clock()->now().nanoseconds()); + auto message = grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap()); + grid_map_pub->publish(std::move(message)); + publishTrajectory(path_pub, path.position()); + publishTrajectory(interpolate_path_pub, interpolated_path); + publishPathSegments(path_segment_pub, path); + publishPositionSetpoints(start_pos_pub, start, start_vel); + publishPositionSetpoints(goal_pos_pub, goal, goal_vel); + publishTree(trajectory_pub, planner->getPlannerData(), planner->getProblemSetup()); + if (!random) { + count_experiments = num_experiments; + // break; + } + // rclcpp::sleep_for(1s); + } } - ros::Duration(1.0).sleep(); - } - ros::spin(); + + private: + + std::string map_path; + std::string color_file_path; + bool random{false}; + + std::shared_ptr terrain_map; + Path path; + std::vector interpolated_path; + double terrain_altitude{100.0}; + + int num_experiments{20}; + int count_experiments{0}; + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr start_pos_pub; + rclcpp::Publisher::SharedPtr goal_pos_pub; + rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr interpolate_path_pub; + rclcpp::Publisher::SharedPtr path_segment_pub; + rclcpp::Publisher::SharedPtr grid_map_pub; + rclcpp::Publisher::SharedPtr trajectory_pub; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto ompl_rrt_planner = std::make_shared(); + rclcpp::spin(ompl_rrt_planner); + rclcpp::shutdown(); return 0; } diff --git a/terrain_planner/src/visualization.cpp b/terrain_planner/src/visualization.cpp new file mode 100644 index 00000000..5797708e --- /dev/null +++ b/terrain_planner/src/visualization.cpp @@ -0,0 +1,204 @@ +#include "terrain_planner/visualization.h" + +void publishCandidateManeuvers( + rclcpp::Publisher::SharedPtr pub, + const std::vector& candidate_maneuvers, + bool visualize_invalid_trajectories) { + visualization_msgs::msg::MarkerArray msg; + + std::vector marker; + visualization_msgs::msg::Marker mark; + mark.action = visualization_msgs::msg::Marker::DELETEALL; + marker.push_back(mark); + msg.markers = marker; + pub->publish(msg); + + std::vector maneuver_library_vector; + int i = 0; + for (auto maneuver : candidate_maneuvers) { + if (maneuver.valid() || visualize_invalid_trajectories) { + maneuver_library_vector.insert(maneuver_library_vector.begin(), trajectory2MarkerMsg(maneuver, i)); + } + i++; + } + msg.markers = maneuver_library_vector; + pub->publish(msg); +} + +void publishPositionSetpoints( + rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d& position, + const Eigen::Vector3d& velocity, + const Eigen::Vector3d scale) { + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Clock().now(); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.header.frame_id = "map"; + marker.id = 0; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + pub->publish(marker); + + marker.header.stamp = rclcpp::Clock().now(); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = scale(0); + marker.scale.y = scale(1); + marker.scale.z = scale(2); + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.pose.position = tf2::toMsg(position); + tf2::Quaternion q; + q.setRPY(0, 0, std::atan2(velocity.y(), velocity.x())); + marker.pose.orientation = tf2::toMsg(q); + + pub->publish(marker); +} + +void publishPath( + rclcpp::Publisher::SharedPtr pub, + std::vector path, + Eigen::Vector3d color) { + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Clock().now(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.header.frame_id = "map"; + marker.id = 0; + marker.action = visualization_msgs::msg::Marker::ADD; + std::vector points; + for (auto& position : path) { + geometry_msgs::msg::Point point; + point.x = position(0); + point.y = position(1); + point.z = position(2); + points.push_back(point); + } + std::cout << "Points: " << points.size() << std::endl; + marker.points = points; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 10.0; + marker.scale.y = 10.0; + marker.scale.z = 10.0; + marker.color.a = 0.8; + marker.color.r = color.x(); + marker.color.g = color.y(); + marker.color.b = color.z(); + pub->publish(marker); +} + +void publishTrajectory( + rclcpp::Publisher::SharedPtr pub, + std::vector trajectory) { + nav_msgs::msg::Path msg; + std::vector posestampedhistory_vector; + Eigen::Vector4d orientation(1.0, 0.0, 0.0, 0.0); + for (auto pos : trajectory) { + posestampedhistory_vector.insert(posestampedhistory_vector.begin(), vector3d2PoseStampedMsg(pos, orientation)); + } + msg.header.stamp = rclcpp::Clock().now(); + msg.header.frame_id = "map"; + msg.poses = posestampedhistory_vector; + pub->publish(msg); +} + +void publishTree( + rclcpp::Publisher::SharedPtr pub, + std::shared_ptr planner_data, + std::shared_ptr problem_setup) { + visualization_msgs::msg::MarkerArray marker_array; + std::vector marker; + + planner_data->decoupleFromPlanner(); + + // Create states, a marker and a list to store edges + ompl::base::ScopedState vertex(problem_setup->getSpaceInformation()); + ompl::base::ScopedState neighbor_vertex( + problem_setup->getSpaceInformation()); + size_t marker_idx{0}; + auto dubins_ss = std::make_shared(); + for (size_t i = 0; i < planner_data->numVertices(); i++) { + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Clock().now(); + marker.header.frame_id = "map"; + vertex = planner_data->getVertex(i).getState(); + marker.ns = "vertex"; + marker.id = marker_idx++; + marker.pose.position.x = vertex[0]; + marker.pose.position.y = vertex[1]; + marker.pose.position.z = vertex[2]; + marker.pose.orientation.w = std::cos(0.5 * vertex[3]); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = std::sin(0.5 * vertex[3]); + marker.scale.x = 10.0; + marker.scale.y = 2.0; + marker.scale.z = 2.0; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker_array.markers.push_back(marker); + + // allocate variables + std::vector edge_list; + int num_edges = planner_data->getEdges(i, edge_list); + if (num_edges > 0) { + for (unsigned int edge : edge_list) { + visualization_msgs::msg::Marker edge_marker; + edge_marker.header.stamp = rclcpp::Clock().now(); + edge_marker.header.frame_id = "map"; + edge_marker.id = marker_idx++; + edge_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + edge_marker.ns = "edge"; + neighbor_vertex = planner_data->getVertex(edge).getState(); + // points.push_back(toMsg(Eigen::Vector3d(vertex[0], vertex[1], vertex[2]))); + // points.push_back(toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); + ompl::base::State* state = dubins_ss->allocState(); + ompl::base::State* from = dubins_ss->allocState(); + from->as()->setX(vertex[0]); + from->as()->setY(vertex[1]); + from->as()->setZ(vertex[2]); + from->as()->setYaw(vertex[3]); + + ompl::base::State* to = dubins_ss->allocState(); + to->as()->setX(neighbor_vertex[0]); + to->as()->setY(neighbor_vertex[1]); + to->as()->setZ(neighbor_vertex[2]); + to->as()->setYaw(neighbor_vertex[3]); + if (dubins_ss->equalStates(from, to)) { + continue; + } + std::vector points; + for (double t = 0.0; t < 1.0; t += 0.02) { + dubins_ss->interpolate(from, to, t, state); + auto interpolated_state = + Eigen::Vector3d(state->as()->getX(), + state->as()->getY(), + state->as()->getZ()); + points.push_back(tf2::toMsg(interpolated_state)); + } + points.push_back(tf2::toMsg(Eigen::Vector3d(neighbor_vertex[0], neighbor_vertex[1], neighbor_vertex[2]))); + edge_marker.points = points; + edge_marker.action = visualization_msgs::msg::Marker::ADD; + edge_marker.pose.orientation.w = 1.0; + edge_marker.pose.orientation.x = 0.0; + edge_marker.pose.orientation.y = 0.0; + edge_marker.pose.orientation.z = 0.0; + edge_marker.scale.x = 1.0; + edge_marker.scale.y = 1.0; + edge_marker.scale.z = 1.0; + edge_marker.color.a = 0.5; // Don't forget to set the alpha! + edge_marker.color.r = 1.0; + edge_marker.color.g = 1.0; + edge_marker.color.b = 0.0; + marker_array.markers.push_back(edge_marker); + } + } + } + pub->publish(marker_array); +} diff --git a/terrain_planner/test/main.cpp b/terrain_planner/test/main.cpp index 4d820af7..92739fb9 100644 --- a/terrain_planner/test/main.cpp +++ b/terrain_planner/test/main.cpp @@ -1,5 +1,6 @@ #include + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/terrain_planner_benchmark/COLCON_IGNORE b/terrain_planner_benchmark/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp index 8c44691f..9a886408 100644 --- a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp +++ b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp @@ -57,12 +57,12 @@ void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, const double radius) { visualization_msgs::Marker marker; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = rclcpp::Clock().now(); marker.type = visualization_msgs::Marker::LINE_STRIP; marker.action = visualization_msgs::Marker::ADD; marker.header.frame_id = "map"; marker.id = 0; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = rclcpp::Clock().now(); std::vector points; for (double t = 0.0; t <= 1.0; t += 0.02) { geometry_msgs::Point point; @@ -226,7 +226,7 @@ int main(int argc, char** argv) { path.appendSegment(goal_loiter_path); // Repeatedly publish results - terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec()); + terrain_map->getGridMap().setTimestamp(rclcpp::Clock().now().toNSec()); grid_map_msgs::GridMap message; grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap(), message); grid_map_pub.publish(message); diff --git a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp index e7c98e34..0a4611db 100644 --- a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp +++ b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp @@ -210,7 +210,7 @@ int main(int argc, char** argv) { } } - terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec()); + terrain_map->getGridMap().setTimestamp(rclcpp::Clock().now().toNSec()); grid_map_msgs::GridMap message; grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap(), message); grid_map_pub.publish(message);