diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index f212425ed60..6682b20369e 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -541,13 +541,23 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ``.enabled | true | Whether it is enabled | | ``.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | -## InputAtWaypoint plugin +## PhotoAtWaypoint plugin -* ``: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypoint` plugin. +* ``: Name corresponding to the `nav2_waypoint_follower::PhotoAtWaypoint` plugin. | Parameter | Default | Description | | ----------| --------| ------------| | ``.enabled | true | Whether it is enabled | +| ``.image_topic | "/camera/color/image_raw" | Camera image topic name to susbcribe | +| ``.save_dir | "/tmp/waypoint_images" | Path to directory to save taken photos | +| ``.image_format | "png" | Desired image format A few other options; "jpeg" , "jpg", "pgm", "tiff" | + +## InputAtWaypoint plugin + +* ``: Name corresponding to the `nav2_waypoint_follower::InputAtWaypoint` plugin. + +| Parameter | Default | Description | +| ----------| --------| ------------| | ``.timeout | 10.0 | Amount of time in seconds to wait for user input before moving on to the next waypoint. | | ``.input_topic | "input_at_waypoint/input" | Topic input is published to to indicate to move to the next waypoint, in `std_msgs/Empty`. | diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index b6925e9f691..3c60d0ae4fb 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -289,8 +289,8 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - waypoint_task_executor_plugin: "waypoint_task_executor" - waypoint_task_executor: + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True - waypoint_pause_duration: 0 + waypoint_pause_duration: 200 diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index a22cd4cd6cc..3253ab3439e 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,6 +1,15 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) +# Try for OpenCV 4.X, but settle for whatever is installed +find_package(OpenCV 4 QUIET) +if(NOT OpenCV_FOUND) + find_package(OpenCV REQUIRED) +endif() +message(STATUS "Found OpenCV version ${OpenCV_VERSION}") + +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) @@ -41,6 +50,9 @@ set(dependencies tf2_ros nav2_core pluginlib + image_transport + cv_bridge + OpenCV ) ament_target_dependencies(${executable_name} @@ -53,16 +65,22 @@ ament_target_dependencies(${library_name} ${dependencies} ) -add_library(simple_waypoint_task_executor SHARED plugins/wait_at_waypoint.cpp) -ament_target_dependencies(simple_waypoint_task_executor ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_waypoint_task_executor PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +add_library(wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp) +ament_target_dependencies(wait_at_waypoint ${dependencies}) +#prevent pluginlib from using boost +target_compile_definitions(wait_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp) +ament_target_dependencies(photo_at_waypoint ${dependencies}) +#prevent pluginlib from using boost +target_compile_definitions(photo_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp) ament_target_dependencies(input_at_waypoint ${dependencies}) +#prevent pluginlib from using boost target_compile_definitions(input_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -install(TARGETS ${library_name} simple_waypoint_task_executor input_at_waypoint +install(TARGETS ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -82,7 +100,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(simple_waypoint_task_executor input_at_waypoint) +ament_export_libraries(wait_at_waypoint photo_at_waypoint input_at_waypoint) pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp new file mode 100644 index 00000000000..7d264b7181e --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2020 Fetullah Atas +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "sensor_msgs/msg/image.hpp" +#include "nav2_core/waypoint_task_executor.hpp" +#include "opencv4/opencv2/core.hpp" +#include "opencv4/opencv2/opencv.hpp" +#include "cv_bridge/cv_bridge.h" +#include "image_transport/image_transport.hpp" + + +namespace nav2_waypoint_follower +{ + +class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor +{ +public: + /** + * @brief Construct a new Photo At Waypoint object + * + */ + PhotoAtWaypoint(); + + /** + * @brief Destroy the Photo At Waypoint object + * + */ + ~PhotoAtWaypoint(); + + /** + * @brief declares and loads parameters used + * + * @param parent parent node that plugin will be created within + * @param plugin_name should be provided in nav2_params.yaml==> waypoint_follower + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + * @return true if task execution was successful + * @return false if task execution failed + */ + bool processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); + + /** + * @brief + * + * @param msg + */ + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg); + + /** + * @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat + * + * @param msg + * @param mat + */ + static void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat); + +protected: + // to ensure safety when accessing global var curr_frame_ + std::mutex global_mutex_; + // the taken photos will be saved under this directory + std::experimental::filesystem::path save_dir_; + // .png ? .jpg ? or some other well known format + std::string image_format_; + // the topic to subscribe in order capture a frame + std::string image_topic_; + // whether plugin is enabled + bool is_enabled_; + // current frame; + sensor_msgs::msg::Image::SharedPtr curr_frame_msg_; + // global logger + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + // ros susbcriber to get camera image + rclcpp::Subscription::SharedPtr camera_image_subscriber_; +}; +} // namespace nav2_waypoint_follower + +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml index 2c38b6c5f2c..ab8c01f050f 100644 --- a/nav2_waypoint_follower/plugins.xml +++ b/nav2_waypoint_follower/plugins.xml @@ -1,9 +1,15 @@ - + Lets robot sleep for a specified amount of time at waypoint arrival + + + Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. + Saves the taken photos to specified directory. + + Lets robot wait for input at waypoint arrival diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp new file mode 100644 index 00000000000..9939d60b680 --- /dev/null +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 Fetullah Atas +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" +#include + +#include +#include + + +namespace nav2_waypoint_follower +{ +PhotoAtWaypoint::PhotoAtWaypoint() +{ +} + +PhotoAtWaypoint::~PhotoAtWaypoint() +{ +} + +void PhotoAtWaypoint::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + + curr_frame_msg_ = std::make_shared(); + + node->declare_parameter(plugin_name + ".enabled", rclcpp::ParameterValue(true)); + node->declare_parameter( + plugin_name + ".image_topic", + rclcpp::ParameterValue("/camera/color/image_raw")); + node->declare_parameter( + plugin_name + ".save_dir", + rclcpp::ParameterValue("/tmp/waypoint_images")); + node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue("png")); + + std::string save_dir_as_string; + node->get_parameter(plugin_name + ".enabled", is_enabled_); + node->get_parameter(plugin_name + ".image_topic", image_topic_); + node->get_parameter(plugin_name + ".save_dir", save_dir_as_string); + node->get_parameter(plugin_name + ".image_format", image_format_); + + // get inputted save directory and make sure it exists, if not log and create it + save_dir_ = save_dir_as_string; + if (!std::experimental::filesystem::exists(save_dir_)) { + RCLCPP_WARN( + logger_, + "Provided save directory for photo at waypoint plugin does not exist," + "provided directory is: %s, the directory will be created automatically.", + save_dir_.c_str() + ); + if (!std::experimental::filesystem::create_directory(save_dir_)) { + RCLCPP_ERROR( + logger_, + "Failed to create directory!: %s required by photo at waypoint plugin, " + "exiting the plugin with failure!", + save_dir_.c_str() + ); + is_enabled_ = false; + } + } + + if (!is_enabled_) { + RCLCPP_INFO( + logger_, "Photo at waypoint plugin is disabled."); + } else { + RCLCPP_INFO( + logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s", + image_topic_.c_str()); + camera_image_subscriber_ = node->create_subscription( + image_topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&PhotoAtWaypoint::imageCallback, this, std::placeholders::_1)); + } +} + +bool PhotoAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) +{ + if (!is_enabled_) { + RCLCPP_WARN( + logger_, + "Photo at waypoint plugin is disabled. Not performing anything" + ); + return true; + } + try { + // construct the full path to image filename + std::experimental::filesystem::path file_name = std::to_string( + curr_waypoint_index) + "_" + + std::to_string(curr_pose.header.stamp.sec) + "." + image_format_; + std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name; + + // save the taken photo at this waypoint to given directory + std::lock_guard guard(global_mutex_); + cv::Mat curr_frame_mat; + deepCopyMsg2Mat(curr_frame_msg_, curr_frame_mat); + cv::imwrite(full_path_image_path.c_str(), curr_frame_mat); + RCLCPP_INFO( + logger_, + "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index); + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, + "Couldn't take photo at waypoint %i! Caught exception: %s \n" + "Make sure that the image topic named: %s is valid and active!", + curr_waypoint_index, + e.what(), image_topic_.c_str()); + return false; + } + return true; +} + +void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + std::lock_guard guard(global_mutex_); + curr_frame_msg_ = msg; +} + +void PhotoAtWaypoint::deepCopyMsg2Mat( + const sensor_msgs::msg::Image::SharedPtr & msg, + cv::Mat & mat) +{ + cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(msg, msg->encoding); + cv::Mat frame = cv_bridge_ptr->image; + if (msg->encoding == "rgb8") { + cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR); + } + frame.copyTo(mat); +} + +} // namespace nav2_waypoint_follower +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::PhotoAtWaypoint, + nav2_core::WaypointTaskExecutor) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 5c0c8955b02..ff81e7666dd 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -35,7 +35,7 @@ WaypointFollower::WaypointFollower() declare_parameter("loop_rate", 20); nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), - rclcpp::ParameterValue(std::string("waypoint_task_executor"))); + rclcpp::ParameterValue(std::string("wait_at_waypoint"))); nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin.plugin"), rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint")));