Nico SW Turtlesim PR

CMakeLists.txt

@@ -0,0 +1,162 @@
+cmake_minimum_required(VERSION 3.5)
+project(software_training_assignment)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(turtlesim REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# setup interfaces
+# set(srv_files
+# 	""
+# )
+
+set (msg_files
+	"msg/Distances.msg"
+)
+
+set(action_files
+	"action/Move.action"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+	${msg_files}
+	${action_files}
+	DEPENDENCIES std_msgs geometry_msgs builtin_interfaces
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+#include the 'include' directory
+include_directories(include)
+
+#create resource which references the libraries in the binary bin
+set(node_plugins "")
+
+#add plugins as SHARED library 
+
+#add clear_turtle_node as a plugin
+add_library(clear_request SHARED 
+	src/clear_turtles.cpp)
+target_compile_definitions(clear_request
+	PRIVATE "SOFTWARE_TRAINING_DLL")
+ament_target_dependencies(clear_request
+	"rclcpp"
+	"rclcpp_components"
+	"turtlesim"
+	"std_srvs")
+# another way of registering and setting the node as a component below
+# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear)
+rclcpp_components_register_nodes(clear_request "composition::clear_turtles")
+# this way we can execute the component with - ros2 component standalone software_training_assignment composition::clear_turtles
+set(node_plugins "${node_plugins}composition::clear_turtles;$<TARGET_FILE:clear_request>\n")
+
+add_library(spawn_turtles SHARED 
+	src/spawn_turtles.cpp)
+target_compile_definitions(spawn_turtles
+	PRIVATE "SOFTWARE_TRAINING_DLL")
+ament_target_dependencies(spawn_turtles
+	"rclcpp"
+	"rclcpp_components"
+	"turtlesim"
+	"std_srvs")
+# another way of registering and setting the node as a component below
+# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear)
+rclcpp_components_register_nodes(spawn_turtles "composition::SpawnTwoTurtles")
+# this way we can execute the component with - ros2 component standalone software_training_assignment composition::SpawnTwoTurtles
+set(node_plugins "${node_plugins}composition::SpawnTwoTurtles;$<TARGET_FILE:spawn_turtles>\n")
+
+
+add_library(move_circular SHARED 
+	src/move_circular.cpp)
+target_compile_definitions(move_circular
+	PRIVATE "SOFTWARE_TRAINING_DLL")
+ament_target_dependencies(move_circular
+	"rclcpp"
+	"rclcpp_components"
+	"turtlesim"
+	"std_srvs"
+	"geometry_msgs"
+	"std_msgs")
+# another way of registering and setting the node as a component below
+# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear)
+rclcpp_components_register_nodes(move_circular "composition::CircularMotion")
+# this way we can execute the component with - ros2 component standalone software_training_assignment composition::SpawnTwoTurtles
+set(node_plugins "${node_plugins}composition::CircularMotion;$<TARGET_FILE:move_circular>\n")
+
+add_library(reset_moving SHARED 
+	src/reset_moving_turtle.cpp)
+target_compile_definitions(reset_moving
+	PRIVATE "SOFTWARE_TRAINING_DLL")
+ament_target_dependencies(reset_moving
+	"rclcpp"
+	"rclcpp_components"
+	"turtlesim"
+	"std_srvs")
+# another way of registering and setting the node as a component below
+# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear)
+rclcpp_components_register_nodes(reset_moving "composition::MotionReset")
+# this way we can execute the component with - ros2 component standalone software_training_assignment composition::SpawnTwoTurtles
+set(node_plugins "${node_plugins}composition::MotionReset;$<TARGET_FILE:reset_moving>\n")
+
+
+add_library(distance_tracker SHARED 
+	src/distance.cpp)
+target_compile_definitions(distance_tracker
+	PRIVATE "SOFTWARE_TRAINING_DLL")
+ament_target_dependencies(distance_tracker
+	"rclcpp"
+	"rclcpp_components"
+	"turtlesim")
+# build ROS custom messages
+rosidl_target_interfaces(distance_tracker ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(distance_tracker "composition::DistanceInfo")
+set(node_plugins "${node_plugins}composition::DistanceInfo;$<TARGET_FILE:distance_tracker>\n")
+
+add_library(move_action_server SHARED 
+	src/move_action.cpp)
+target_compile_definitions(move_action_server
+	PRIVATE "SOFTWARE_TRAINING_DLL")
+ament_target_dependencies(move_action_server
+	"rclcpp"
+	"rclcpp_components"
+	"turtlesim"
+	"rclcpp_action"
+	"geometry_msgs")
+# build ROS custom messages
+rosidl_target_interfaces(move_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(move_action_server "composition::MoveAction")
+set(node_plugins "${node_plugins}composition::MoveAction;$<TARGET_FILE:move_action_server>\n")
+
+
+#tell where to put binaries
+install(TARGETS
+	clear_request
+	spawn_turtles
+	move_circular
+	reset_moving
+	distance_tracker
+	move_action_server
+	ARCHIVE DESTINATION lib
+	LIBRARY DESTINATION lib 
+	RUNTIME DESTINATION bin)
+
+ament_package()

action/Move.action

@@ -0,0 +1,9 @@
+# req
+float32 x_pos
+float32 y_pos
+---
+# res
+uint64 time_taken
+---
+# feedback
+float32 dist_to_goal

include/software_training_assignment/clear_turtles.hpp

@@ -0,0 +1,34 @@
+// #ifndef CLEAR_TURTLES_HPP_
+// #define CLEAR_TURTLES_HPP_
+
+// #include <cstdlib>
+// #include <memory>
+// #include <string>
+// #include <vector>
+
+// #include <rclcpp/rclcpp.hpp>
+
+// #include <software_training/visibility.h>
+// #include <turtlesim/srv/kill.hpp>
+
+// namespace composition {
+
+// class turtle_service_request_node : public rclcpp::Node {
+// public:
+//   SOFTWARE_TRAINING_PUBLIC
+//   explicit turtle_service_request_node(const rclcpp::NodeOptions &options);
+
+// private:
+//   rclcpp::Client<turtlesim::srv::Kill>::SharedPtr client;
+//   rclcpp::TimerBase::SharedPtr timer;
+
+//   // all the turtles
+//   std::vector<std::string> turtle_names = {"turtle1", "moving_turtle",
+//                                            "stationary_turtle"};
+
+//   SOFTWARE_TRAINING_LOCAL
+//   void kill();
+// };
+
+// } // namespace composition
+// #endif // TURTLE_SERVICE_REQUEST_NODE_HPP_

include/software_training_assignment/common.hpp

@@ -0,0 +1,11 @@
+#ifndef COMMON_H
+#define COMMON_H
+
+// define common structs here
+struct Position {
+    float x;
+    float y;
+    float theta;
+};
+
+#endif

include/software_training_assignment/distance.hpp

@@ -0,0 +1,59 @@
+#ifndef DISTANCE_H
+#define DISTANCE_H
+
+#include <memory>
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <software_training_assignment/visibility.h>
+#include <turtlesim/msg/pose.hpp>
+#include <software_training_assignment/msg/distances.hpp>
+#include <software_training_assignment/common.hpp>
+
+// using namespace std::chrono_literals;
+// using namespace std::placeholders;
+
+
+namespace composition {
+    class DistanceInfo: public rclcpp::Node {
+    public: 
+        using pose_msg = turtlesim::msg::Pose;
+        using distance_msg = software_training_assignment::msg::Distances;
+
+        SOFTWARE_TRAINING_PUBLIC
+        explicit DistanceInfo(const rclcpp::NodeOptions &options);
+
+    private:
+        rclcpp::Publisher<distance_msg>::SharedPtr distance_pub;
+        rclcpp::TimerBase::SharedPtr pub_timer;
+        rclcpp::Subscription<pose_msg>::SharedPtr moving_pos_sub;
+        rclcpp::Subscription<pose_msg>::SharedPtr static_pos_sub;
+
+        // create threads to run callbacks
+        rclcpp::CallbackGroup::SharedPtr callbacks;
+
+        // struct Position {
+        //     float x;
+        //     float y;
+        //     float theta;
+        // };
+
+        Position moving_turtle_pos;
+        Position static_turtle_pos;
+        bool read_moving, read_static;
+
+        SOFTWARE_TRAINING_LOCAL
+        // timer call back to get current positions and compute distances
+        void publish_distances();
+        // subscriber functions to get data from topic
+        void get_moving_position(const pose_msg::SharedPtr msg);
+        void get_static_position(const pose_msg::SharedPtr msg);
+
+        // helper function for computing euclidean distance
+        double compute_euc_dist();
+
+    };
+}
+
+#endif

include/software_training_assignment/move_action.hpp

@@ -0,0 +1,56 @@
+#ifndef DISTANCE_H
+#define DISTANCE_H
+
+#include <memory>
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include <turtlesim/msg/pose.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+
+#include <software_training_assignment/visibility.h>
+#include <software_training_assignment/common.hpp>
+#include "software_training_assignment/action/move.hpp"
+
+
+namespace composition {
+class MoveAction: public rclcpp::Node {
+public: 
+    using motion_action = software_training_assignment::action::Move;
+    using motion_server = rclcpp_action::Server<motion_action>;
+    using goal_handle_ns = rclcpp_action::ServerGoalHandle<motion_action>;
+    using pose_msg = turtlesim::msg::Pose;
+    using twist_msg = geometry_msgs::msg::Twist;
+    SOFTWARE_TRAINING_PUBLIC
+    explicit MoveAction(const rclcpp::NodeOptions &options);
+
+private:
+    motion_server::SharedPtr move_server;
+    rclcpp::Publisher<twist_msg>::SharedPtr do_move_pub;
+    rclcpp::Subscription<pose_msg>::SharedPtr pos_sub;
+
+    Position curr_pos;
+    Position goal_pos;
+    // constant values to play around with
+    const float SMALL_DIST = 0.1;
+    const float MOVE_SCALE = 0.5;
+
+    void get_pos(const pose_msg::SharedPtr msg);
+    float euc_dist();
+
+    rclcpp_action::GoalResponse handle_goal(
+        const rclcpp_action::GoalUUID &uuid,
+        std::shared_ptr<const motion_action::Goal> goal);
+
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<goal_handle_ns> goal_handle);
+
+    void handle_accepted(const std::shared_ptr<goal_handle_ns> goal_handle);
+
+    void execute(const std::shared_ptr<goal_handle_ns> goal_handle);
+
+};
+}
+
+#endif