Software Training Cindy Li

.gitignore

@@ -0,0 +1 @@
+.vscode

software_training/CMakeLists.txt

@@ -0,0 +1,148 @@
+cmake_minimum_required(VERSION 3.5)
+project(software_training)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# 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(rclcpp_components REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(turtlesim REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+include_directories(include)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/Success.srv"
+  "msg/Distance.msg"
+  "action/Waypoint.action"
+)
+ament_export_dependencies(rosidl_default_runtime)
+
+set(node_plugins "")
+
+# clear turtles service component
+add_library(clear_turtles_server SHARED
+  src/clear_turtles_server.cpp)
+target_compile_definitions(clear_turtles_server
+  PRIVATE "SOFTWARE_TRAINING_BUILDING_DLL")
+ament_target_dependencies(clear_turtles_server
+  "rclcpp"
+  "rclcpp_components"
+  "turtlesim"
+)
+rosidl_target_interfaces(clear_turtles_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(clear_turtles_server "software_training::clear_turtles_server")
+set(node_plugins "${node_plugins}software_training::clear_turtles_server;$<TARGET_FILE:clear_turtles_server>\n")
+
+# move turtle1 in circle publisher component
+add_library(move_turtle1_in_circle_publisher SHARED
+  src/move_turtle1_in_circle_publisher.cpp)
+target_compile_definitions(move_turtle1_in_circle_publisher
+  PRIVATE "SOFTWARE_TRAINING_BUILDING_DLL")
+ament_target_dependencies(move_turtle1_in_circle_publisher
+  "rclcpp"
+  "rclcpp_components"
+  "turtlesim"
+  "geometry_msgs"
+)
+rosidl_target_interfaces(move_turtle1_in_circle_publisher ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(move_turtle1_in_circle_publisher "software_training::move_turtle1_in_circle_publisher")
+set(node_plugins "${node_plugins}software_training::move_turtle1_in_circle_publisher;$<TARGET_FILE:move_turtle1_in_circle_publisher>\n")
+
+# spawn turtles service component
+add_library(spawn_turtles_server SHARED
+  src/spawn_turtles_server.cpp)
+target_compile_definitions(spawn_turtles_server
+  PRIVATE "SOFTWARE_TRAINING_BUILDING_DLL")
+ament_target_dependencies(spawn_turtles_server
+  "rclcpp"
+  "rclcpp_components"
+  "turtlesim"
+)
+rosidl_target_interfaces(spawn_turtles_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(spawn_turtles_server "software_training::spawn_turtles_server")
+set(node_plugins "${node_plugins}software_training::spawn_turtles_server;$<TARGET_FILE:spawn_turtles_server>\n")
+
+# reset moving turtle service component
+add_library(reset_moving_turtle_server SHARED
+  src/reset_moving_turtle_server.cpp)
+target_compile_definitions(reset_moving_turtle_server
+  PRIVATE "SOFTWARE_TRAINING_BUILDING_DLL")
+ament_target_dependencies(reset_moving_turtle_server
+  "rclcpp"
+  "rclcpp_components"
+  "turtlesim"
+)
+rosidl_target_interfaces(reset_moving_turtle_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(reset_moving_turtle_server "software_training::reset_moving_turtle_server")
+set(node_plugins "${node_plugins}software_training::reset_moving_turtle_server;$<TARGET_FILE:reset_moving_turtle_server>\n")
+
+# turtle distance publisher component
+add_library(turtle_distance_publisher SHARED
+  src/turtle_distance_publisher.cpp)
+target_compile_definitions(turtle_distance_publisher
+  PRIVATE "SOFTWARE_TRAINING_BUILDING_DLL")
+ament_target_dependencies(turtle_distance_publisher
+  "rclcpp"
+  "rclcpp_components"
+  "turtlesim"
+)
+rosidl_target_interfaces(turtle_distance_publisher ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(turtle_distance_publisher "software_training::turtle_distance_publisher")
+set(node_plugins "${node_plugins}software_training::turtle_distance_publisher;$<TARGET_FILE:turtle_distance_publisher>\n")
+
+# move to waypoint action service component
+add_library(move_to_waypoint_action_server SHARED
+  src/move_to_waypoint_action_server.cpp)
+target_compile_definitions(move_to_waypoint_action_server
+  PRIVATE "SOFTWARE_TRAINING_BUILDING_DLL")
+ament_target_dependencies(move_to_waypoint_action_server
+  "rclcpp"
+  "rclcpp_components"
+  "rclcpp_action"
+  "turtlesim"
+  "geometry_msgs"
+)
+rosidl_target_interfaces(move_to_waypoint_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rclcpp_components_register_nodes(move_to_waypoint_action_server "software_training::move_to_waypoint_action_server")
+set(node_plugins "${node_plugins}software_training::move_to_waypoint_action_server;$<TARGET_FILE:move_to_waypoint_action_server>\n")
+
+install(TARGETS
+  clear_turtles_server
+  move_turtle1_in_circle_publisher
+  spawn_turtles_server
+  reset_moving_turtle_server
+  turtle_distance_publisher
+  move_to_waypoint_action_server
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

software_training/action/Waypoint.action

@@ -0,0 +1,6 @@
+float32 waypoint_x
+float32 waypoint_y
+---
+float32 time_s
+---
+float32 distance_to_waypoint

software_training/include/software_training/clear_turtles_server.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <chrono>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "turtlesim/srv/kill.hpp"
+#include "software_training/srv/success.hpp"
+#include "software_training/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace software_training {
+
+class clear_turtles_server : public rclcpp::Node {
+
+    public:
+        SOFTWARE_TRAINING_PUBLIC
+        explicit clear_turtles_server(const rclcpp::NodeOptions& options);
+
+    private:
+        std::vector<std::string> TURTLE_NAMES = {"turtle1", "moving_turtle", "stationary_turtle"};
+
+        rclcpp::Service<software_training::srv::Success>::SharedPtr service;
+        rclcpp::Client<turtlesim::srv::Kill>::SharedPtr kill_turtles_client;
+
+        // Service to clear all existing turtles
+        SOFTWARE_TRAINING_LOCAL
+        void clear_turtles(const std::shared_ptr<software_training::srv::Success::Request> request,
+                           std::shared_ptr<software_training::srv::Success::Response> response);
+};
+}

software_training/include/software_training/move_to_waypoint_action_server.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <chrono>
+#include <cmath>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "turtlesim/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "software_training/action/waypoint.hpp"
+#include "software_training/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace software_training {
+
+class move_to_waypoint_action_server : public rclcpp::Node {
+
+    public:
+        using Waypoint = software_training::action::Waypoint;
+        using GoalHandleWaypoint = rclcpp_action::ServerGoalHandle<Waypoint>;
+
+        SOFTWARE_TRAINING_PUBLIC
+        explicit move_to_waypoint_action_server(const rclcpp::NodeOptions& options);
+
+    private:
+
+        static constexpr float FLOAT_TOLERANCE = 0.05;
+        static constexpr float LOOP_RATE_HZ = 100;
+
+        typedef struct {
+            float x;
+            float y;
+            float theta;
+        } turtle_pose;
+
+        turtle_pose moving_turtle_pose = {0, 0, 0};
+
+        rclcpp_action::Server<Waypoint>::SharedPtr server;
+        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher;
+        rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr moving_turtle_subscription;
+
+        SOFTWARE_TRAINING_LOCAL
+        void subscription_callback(const turtlesim::msg::Pose::SharedPtr msg);
+
+        SOFTWARE_TRAINING_LOCAL
+        rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Waypoint::Goal> goal);
+
+        SOFTWARE_TRAINING_LOCAL
+        rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleWaypoint> goal_handle);
+
+        SOFTWARE_TRAINING_LOCAL
+        void handle_accepted(const std::shared_ptr<GoalHandleWaypoint> goal_handle);
+
+        SOFTWARE_TRAINING_LOCAL
+        void execute(const std::shared_ptr<GoalHandleWaypoint> goal_handle);
+
+        SOFTWARE_TRAINING_LOCAL
+        float distance_to_point(float x, float y);
+};
+
+}

software_training/include/software_training/move_turtle1_in_circle_publisher.hpp

@@ -0,0 +1,25 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "turtlesim/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "software_training/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace software_training {
+
+class move_turtle1_in_circle_publisher : public rclcpp::Node {
+
+    public:
+        SOFTWARE_TRAINING_PUBLIC
+        explicit move_turtle1_in_circle_publisher(const rclcpp::NodeOptions& options);
+
+    private:
+        rclcpp::TimerBase::SharedPtr timer;
+        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher;
+
+        SOFTWARE_TRAINING_LOCAL
+        void publish(void);
+};
+}

software_training/include/software_training/reset_moving_turtle_server.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "turtlesim/srv/teleport_absolute.hpp"
+#include "software_training/srv/success.hpp"
+#include "software_training/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace software_training {
+
+class reset_moving_turtle_server : public rclcpp::Node {
+
+    public:
+        SOFTWARE_TRAINING_PUBLIC
+        explicit reset_moving_turtle_server(const rclcpp::NodeOptions& options);
+
+    private:
+        static constexpr float START_X = 25, START_Y = 10;
+
+        rclcpp::Service<software_training::srv::Success>::SharedPtr service;
+        rclcpp::Client<turtlesim::srv::TeleportAbsolute>::SharedPtr teleport_turtle_client;
+
+        // Service to reset moving_turtle to its starting position
+        SOFTWARE_TRAINING_LOCAL
+        void reset_moving_turtle(const std::shared_ptr<software_training::srv::Success::Request> request,
+                                 std::shared_ptr<software_training::srv::Success::Response> response);
+};
+}

software_training/include/software_training/spawn_turtles_server.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <chrono>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "turtlesim/srv/spawn.hpp"
+#include "software_training/srv/success.hpp"
+#include "software_training/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace software_training {
+
+class spawn_turtles_server : public rclcpp::Node {
+
+    public:
+        SOFTWARE_TRAINING_PUBLIC
+        explicit spawn_turtles_server(const rclcpp::NodeOptions& options);
+
+    private:
+
+        typedef struct {
+            std::string name;
+            float x;
+            float y;
+        } turtle_descriptor;
+
+        std::vector<turtle_descriptor> turtle_descriptions = {
+            {"stationary_turtle", 5, 5},
+            {"moving_turtle", 25, 10}
+        };
+
+        rclcpp::Service<software_training::srv::Success>::SharedPtr service;
+        rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawn_turtle_client;
+
+        // Service to spawn stationary_turtle and moving_turtle
+        SOFTWARE_TRAINING_LOCAL
+        void spawn_turtles(const std::shared_ptr<software_training::srv::Success::Request> request,
+                           std::shared_ptr<software_training::srv::Success::Response> response);
+};
+}

software_training/include/software_training/turtle_distance_publisher.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "turtlesim/msg/pose.hpp"
+#include "software_training/msg/distance.hpp"
+#include "software_training/visibility_control.h"
+
+using namespace std::chrono_literals;
+
+namespace software_training {
+
+class turtle_distance_publisher : public rclcpp::Node {
+
+    public:
+        SOFTWARE_TRAINING_PUBLIC
+        explicit turtle_distance_publisher(const rclcpp::NodeOptions& options);
+
+    private:
+        rclcpp::TimerBase::SharedPtr timer;
+        rclcpp::Publisher<software_training::msg::Distance>::SharedPtr publisher;
+        rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr stationary_turtle_subscription;
+        rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr moving_turtle_subscription;
+
+        typedef struct {
+            float x;
+            float y;
+        } turtle_position;
+
+        turtle_position stationary_turtle_position = {0, 0};
+        turtle_position moving_turtle_position = {0, 0};
+
+        SOFTWARE_TRAINING_LOCAL
+        void publish(void);
+
+        SOFTWARE_TRAINING_LOCAL
+        void stationary_turtle_callback(const turtlesim::msg::Pose::SharedPtr msg);
+
+        SOFTWARE_TRAINING_LOCAL
+        void moving_turtle_callback(const turtlesim::msg::Pose::SharedPtr msg);
+};
+}