diff --git a/explore/CMakeLists.txt b/explore/CMakeLists.txt index d506f3e..ccdd448 100644 --- a/explore/CMakeLists.txt +++ b/explore/CMakeLists.txt @@ -15,6 +15,20 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# Set flag depending on distro +if(NOT DEFINED ENV{ROS_DISTRO}) + message(FATAL_ERROR "ROS_DISTRO is not defined." ) +endif() +if("$ENV{ROS_DISTRO}" STREQUAL "eloquent") + message(STATUS "Build for ROS2 eloquent") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT") +elseif("$ENV{ROS_DISTRO}" STREQUAL "dashing") + message(STATUS "Build for ROS2 dashing") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING") +else() + message(STATUS "BuilD for ROS2: " "$ENV{ROS_DISTRO}") +endif() + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -27,7 +41,6 @@ find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(map_msgs REQUIRED) find_package(visualization_msgs REQUIRED) - find_package(nav2_costmap_2d REQUIRED) @@ -63,6 +76,7 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME} ) + add_executable(explore src/costmap_client.cpp src/explore.cpp diff --git a/explore/include/explore/explore.h b/explore/include/explore/explore.h index 4ebd16a..2a93d49 100644 --- a/explore/include/explore/explore.h +++ b/explore/include/explore/explore.h @@ -58,7 +58,13 @@ #include "rclcpp_action/rclcpp_action.hpp" using namespace std::placeholders; - +#ifdef ELOQUENT +#define ACTION_NAME "/NavigateToPose" +#elif DASHING +#define ACTION_NAME "/NavigateToPose" +#else +#define ACTION_NAME "/navigate_to_pose" +#endif namespace explore { /** diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp index 8e320b4..b2cd5bd 100644 --- a/explore/src/explore.cpp +++ b/explore/src/explore.cpp @@ -68,12 +68,8 @@ Explore::Explore() this->get_parameter("gain_scale", gain_scale_); this->get_parameter("min_frontier_size", min_frontier_size); progress_timeout_ = timeout; - move_base_client_ = - rclcpp_action::create_client(this, - "/navigat" - "e_to_" - "pose"); + rclcpp_action::create_client(this,ACTION_NAME); search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), potential_scale_, gain_scale_,