Initial commit, not working

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,46 @@
+#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="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>

urc_behaviors/src/search_for_aruco.cpp

@@ -0,0 +1,110 @@
+#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()
+    {
+        // 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)
+    {
+        nav_msgs::msg::Path path;
+        path.header.frame_id = "map";
+        path.header.stamp = this->now();
+
+        // Lookup transform from base_link to map
+        geometry_msgs::msg::TransformStamped transform;
+        try
+        {
+            transform = tf_buffer_->lookupTransform("odom", "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 = path.header;
+
+            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
+            geometry_msgs::msg::PoseStamped transformed_pose;
+            tf2::doTransform(pose, transformed_pose, transform);
+
+            path.poses.push_back(transformed_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_sim.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(
@@ -186,7 +185,6 @@ def generate_launch_description():
                     on_exit=[
                         load_joint_state_broadcaster,
                         load_drivetrain_controller,
-                        teleop_launch,
                         map_to_odom_launch,
                     ],
                 )
@@ -203,7 +201,7 @@ def generate_launch_description():
                     on_start=[
                         path_planning_launch,
                         trajectory_following_launch,
-                        bt_launch,
+                        # bt_launch,
                     ],
                 )
             ),

urc_bringup/strategies/bt_test.xml

@@ -7,8 +7,6 @@
           <LogInfo logger="{ros_log}"
                    message="Trying to Plan Path..."/>
         </Delay>
-        <StatusLightPublisher topic_name="/cmd_statuslight_color" value="1"/>
-        <StatusLightPublisher topic_name="/cmd_statuslight_display" value="1"/>
         <KeepRunningUntilFailure>
           <Inverter>
             <Sequence>
@@ -52,11 +50,6 @@
       <input_port name="message"
                   default="&quot;Replace this with the true message.&quot;">Message to log.</input_port>
     </Action>
-    <Action ID="StatusLightPublisher"
-            editable="true">
-      <input_port name="topic_name"/>
-      <input_port name="value"/>
-    </Action>
     <Action ID="UpdateCurrentPose"
             editable="true">
       <input_port name="topic_name"/>