[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,48 @@
+#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 "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+
+namespace urc_behaviors
+{
+class SearchForAruco : public rclcpp::Node
+{
+public:
+  using FollowPath = urc_msgs::action::FollowPath;
+  using GoalHandleFollowPath = rclcpp_action::ClientGoalHandle<FollowPath>;
+
+  explicit SearchForAruco(const rclcpp::NodeOptions & options);
+
+  ~SearchForAruco() = default;
+
+private:
+  void search();
+
+  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_;
+  rclcpp::Time aruco_first_seen_time_;
+
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+};
+} // 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="[email protected]">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>

urc_behaviors/src/search_for_aruco.cpp

@@ -0,0 +1,122 @@
+#include "search_for_aruco.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace urc_behaviors
+{
+SearchForAruco::SearchForAruco(const rclcpp::NodeOptions & options)
+: Node("search_for_aruco", options)
+{
+  RCLCPP_INFO(this->get_logger(), "Search for ARUCO behavior server has been started.");
+
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  follow_path_client_ = rclcpp_action::create_client<urc_msgs::action::FollowPath>(
+    this,
+    "follow_path");
+
+  search();
+}
+
+void SearchForAruco::search()
+{
+  // Wait until base_link to map transform is available
+  while (!tf_buffer_->canTransform("map", "base_link", tf2::TimePointZero)) {
+    RCLCPP_INFO(this->get_logger(), "Waiting for transform from base_link to map...");
+    rclcpp::sleep_for(std::chrono::seconds(1));
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Transform from base_link to map is available.");
+
+  // Generate a search path
+  auto path = generate_search_path(0.1);
+
+  // Send the path to the follow_path action server
+  auto goal = FollowPath::Goal();
+  goal.path = path;
+
+  auto send_goal_options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
+  send_goal_options.goal_response_callback = std::bind(
+    &SearchForAruco::goal_response_callback,
+    this, std::placeholders::_1);
+  send_goal_options.result_callback = std::bind(
+    &SearchForAruco::result_callback, this,
+    std::placeholders::_1);
+  send_goal_options.feedback_callback = std::bind(
+    &SearchForAruco::feedback_callback, this,
+    std::placeholders::_1, std::placeholders::_2);
+
+  follow_path_client_->async_send_goal(goal, send_goal_options);
+}
+
+void SearchForAruco::goal_response_callback(const GoalHandleFollowPath::SharedPtr & goal_handle)
+{
+  if (!goal_handle) {
+    RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
+  }
+}
+
+void SearchForAruco::result_callback(const GoalHandleFollowPath::WrappedResult & result)
+{
+  switch (result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED:
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+      break;
+    case rclcpp_action::ResultCode::ABORTED:
+      RCLCPP_INFO(this->get_logger(), "Goal was aborted");
+      break;
+    case rclcpp_action::ResultCode::CANCELED:
+      RCLCPP_INFO(this->get_logger(), "Goal was canceled");
+      break;
+    default:
+      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+      break;
+  }
+}
+
+void SearchForAruco::feedback_callback(
+  GoalHandleFollowPath::SharedPtr,
+  const std::shared_ptr<const FollowPath::Feedback> feedback)
+{
+  RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance_to_goal);
+}
+
+nav_msgs::msg::Path SearchForAruco::generate_search_path(double spiral_coeff)
+{
+  rclcpp::Time now = this->now();
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "map";
+  path.header.stamp = now;
+
+  // Lookup transform from base_link to map
+  geometry_msgs::msg::TransformStamped transform;
+  try {
+    transform = tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(this->get_logger(), "Failed to lookup transform: %s", ex.what());
+    throw std::runtime_error("Failed to lookup transform.");
+  }
+
+  for (double t = 0; t < 12 * M_PI; t += M_PI / 4) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = "map";
+    pose.header.stamp = now;
+
+    pose.pose.position.x = spiral_coeff * t * cos(t);
+    pose.pose.position.y = spiral_coeff * t * sin(t);
+    pose.pose.orientation.w = 1.0;
+
+    // Transform pose to map frame
+    tf2::doTransform(pose.pose, pose.pose, transform);
+
+    path.poses.push_back(pose);
+  }
+
+  return path;
+}
+} // namespace urc_behaviors
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(urc_behaviors::SearchForAruco)

urc_bringup/launch/bringup_simulation.launch.py

@@ -29,8 +29,7 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
 
     xacro_file = os.path.join(
-        get_package_share_directory("urc_hw_description"),
-        "urdf/walli.xacro"
+        get_package_share_directory("urc_hw_description"), "urdf/walli.xacro"
     )
     assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file)
     robot_description_config = process_file(
@@ -162,6 +161,12 @@ def generate_launch_description():
         ],
     )
 
+    search_for_aruco_node = Node(
+        package="urc_behaviors",
+        executable="urc_behaviors_SearchForAruco",
+        output="screen",
+    )
+
     map_to_odom_transform_node = Node(
         package="tf2_ros",
         executable="static_transform_publisher",
@@ -186,8 +191,6 @@ def generate_launch_description():
                     on_exit=[
                         load_joint_state_broadcaster,
                         load_drivetrain_controller,
-                        teleop_launch,
-                        map_to_odom_launch,
                     ],
                 )
             ),
@@ -200,17 +203,24 @@ def generate_launch_description():
             RegisterEventHandler(
                 event_handler=OnProcessStart(
                     target_action=elevation_mapping_node,
+                    on_start=[map_to_odom_transform_node],
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessStart(
+                    target_action=map_to_odom_transform_node,
                     on_start=[
+                        map_to_odom_launch,
                         path_planning_launch,
                         trajectory_following_launch,
-                        bt_launch,
+                        search_for_aruco_node,
+                        # bt_launch,
                     ],
                 )
             ),
             enable_color,
             gazebo,
             load_robot_state_publisher,
             spawn_robot,
-            map_to_odom_transform_node,
         ]
     )

urc_navigation/path_planning/include/planner_server.hpp

@@ -24,12 +24,6 @@ class PlannerServer : public rclcpp::Node
     const std::shared_ptr<urc_msgs::srv::GeneratePlan::Request> request,
     std::shared_ptr<urc_msgs::srv::GeneratePlan::Response> response);
 
-  /**
-   * @brief Publish the plan to the /path topic for *visualization* purposes. The plan will be returned as a response to the service call.
-   * @param plan The plan to be published
-   */
-  void publishPlan(const nav_msgs::msg::Path & plan);
-
   /**
    * @brief Wait for the costmap to be available
    */
@@ -43,7 +37,6 @@ class PlannerServer : public rclcpp::Node
 
   nav_msgs::msg::OccupancyGrid current_costmap_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_subscriber_;
-  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
   rclcpp::Service<urc_msgs::srv::GeneratePlan>::SharedPtr plan_service_;
 };
 } // namespace planner_server

urc_navigation/path_planning/src/planner_server.cpp

@@ -16,11 +16,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
     "plan",
     std::bind(&PlannerServer::generatePlan, this, std::placeholders::_1, std::placeholders::_2));
 
-  // Create the publisher
-  plan_publisher_ = create_publisher<nav_msgs::msg::Path>(
-    "/path",
-    rclcpp::SystemDefaultsQoS());
-
   // Setup the costmap
   costmap_subscriber_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
     "/costmap",
@@ -71,21 +66,11 @@ void PlannerServer::generatePlan(
     response->error_code = urc_msgs::srv::GeneratePlan::Response::SUCCESS;
 
     RCLCPP_INFO(get_logger(), "Returning plan with %ld poses", plan.poses.size());
-
-    publishPlan(plan);
   } catch (const std::exception & e) {
     RCLCPP_ERROR(get_logger(), "Error generating plan: %s", e.what());
     response->error_code = urc_msgs::srv::GeneratePlan::Response::FAILURE;
   }
 }
-
-void PlannerServer::publishPlan(const nav_msgs::msg::Path & plan)
-{
-  auto msg = std::make_unique<nav_msgs::msg::Path>(plan);
-  if (plan_publisher_->get_subscription_count() > 0) {
-    plan_publisher_->publish(std::move(msg));
-  }
-}
 } // namespace planner_server
 
 #include <rclcpp_components/register_node_macro.hpp>