[navigation] add behavior to search for aruco tag

urc_behaviors/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.5)
+project(urc_behaviors)
+
+include(../cmake/default_settings.cmake)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(urc_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include_directories(
+  include
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/search_for_aruco.cpp
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  urc_msgs
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  nav_msgs
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${dependencies}
+)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "urc_behaviors::SearchForAruco"
+  EXECUTABLE ${PROJECT_NAME}_SearchForAruco
+)
+
+# Install launch files.
+install(
+  DIRECTORY
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(msg)
+
+ament_export_include_directories(include)
+
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+
+ament_package()

urc_behaviors/include/search_for_aruco.hpp

@@ -0,0 +1,66 @@
+#ifndef SEARCH_FOR_ARUCO_HPP_
+#define SEARCH_FOR_ARUCO_HPP_
+
+#include <future>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <nav_msgs/msg/path.hpp>
+
+#include <urc_msgs/action/follow_path.hpp>
+#include <urc_msgs/action/search_aruco.hpp>
+
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+namespace urc_behaviors
+{
+class SearchForAruco : public rclcpp::Node, public std::enable_shared_from_this<SearchForAruco>
+{
+public:
+  using FollowPath = urc_msgs::action::FollowPath;
+  using SearchAruco = urc_msgs::action::SearchAruco;
+  using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle<FollowPath>;
+  using GoalHandleSearchAruco = rclcpp_action::ServerGoalHandle<urc_msgs::action::SearchAruco>;
+
+  explicit SearchForAruco(const rclcpp::NodeOptions & options);
+
+  ~SearchForAruco() = default;
+
+private:
+  void search(const std::shared_ptr<GoalHandleSearchAruco> goal_handle);
+
+  nav_msgs::msg::Path generate_search_path(double spiral_coeff);
+
+  void goal_response_callback(const GoalHandleFollowPath::SharedPtr & goal_handle);
+  void result_callback(const GoalHandleFollowPath::WrappedResult & result);
+  void feedback_callback(
+    GoalHandleFollowPath::SharedPtr,
+    const std::shared_ptr<const FollowPath::Feedback> feedback);
+
+  rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;
+
+  bool aruco_seen_ = false;
+  bool cancel_requested_ = false;
+  rclcpp::Time aruco_first_seen_time_;
+  void aruco_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg);
+  void handle_accepted(const std::shared_ptr<GoalHandleSearchAruco> goal_handle);
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const urc_msgs::action::SearchAruco::Goal> goal);
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<urc_msgs::action::SearchAruco>> goal_handle);
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  rclcpp_action::Server<urc_msgs::action::SearchAruco>::SharedPtr search_server_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr aruco_seen_subscriber_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr aruco_seen_publisher_;
+};
+
+} // namespace urc_behaviors
+
+#endif // SEARCH_FOR_ARUCO_HPP_

urc_behaviors/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>urc_behaviors</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yashas.amb@gmail.com">yashas</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>urc_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>