From 0133b53cac9c4fb6f03ef53238bb6a825186e422 Mon Sep 17 00:00:00 2001 From: matteoepifani Date: Sun, 25 May 2025 14:27:17 -0400 Subject: [PATCH] initial software training commit --- software_training/CMakeLists.txt | 221 ++++++++++++++++++ software_training/action/MovingTurtle.action | 7 + .../include/software_training/visibility.h | 44 ++++ software_training/launch/launch.py | 61 +++++ software_training/msg/Distance.msg | 3 + software_training/package.xml | 31 +++ software_training/src/clear_turtle.cpp | 64 +++++ software_training/src/distance_publisher.cpp | 63 +++++ software_training/src/distance_subscriber.cpp | 32 +++ .../src/moving_turtle_client.cpp | 99 ++++++++ .../src/moving_turtle_server.cpp | 155 ++++++++++++ software_training/src/reset_moving_turtle.cpp | 72 ++++++ .../src/reset_moving_turtle_client.cpp | 64 +++++ software_training/src/spawn_turtle.cpp | 90 +++++++ software_training/src/turtle1_circle.cpp | 48 ++++ software_training/srv/Reset.srv | 3 + 16 files changed, 1057 insertions(+) create mode 100644 software_training/CMakeLists.txt create mode 100644 software_training/action/MovingTurtle.action create mode 100644 software_training/include/software_training/visibility.h create mode 100644 software_training/launch/launch.py create mode 100644 software_training/msg/Distance.msg create mode 100644 software_training/package.xml create mode 100644 software_training/src/clear_turtle.cpp create mode 100644 software_training/src/distance_publisher.cpp create mode 100644 software_training/src/distance_subscriber.cpp create mode 100644 software_training/src/moving_turtle_client.cpp create mode 100644 software_training/src/moving_turtle_server.cpp create mode 100644 software_training/src/reset_moving_turtle.cpp create mode 100644 software_training/src/reset_moving_turtle_client.cpp create mode 100644 software_training/src/spawn_turtle.cpp create mode 100644 software_training/src/turtle1_circle.cpp create mode 100644 software_training/srv/Reset.srv diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt new file mode 100644 index 0000000..c33eaa2 --- /dev/null +++ b/software_training/CMakeLists.txt @@ -0,0 +1,221 @@ +cmake_minimum_required(VERSION 3.8) +project(software_training) + +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_action REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(turtlesim REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +include_directories(include) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/Reset.srv" + "msg/Distance.msg" + "action/MovingTurtle.action" +) +ament_export_dependencies(rosidl_default_runtime) + +# ---- clear_turtle ---- +add_library(clear_turtle SHARED + src/clear_turtle.cpp) +target_include_directories(clear_turtle PRIVATE + $ + $) +target_compile_definitions(clear_turtle + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(clear_turtle + "std_srvs" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(clear_turtle PLUGIN "software_training::ClearTurtle" EXECUTABLE clear_turtle_node) +install(TARGETS + clear_turtle + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# ---- turtle1_circle ---- +add_library(turtle1_circle SHARED + src/turtle1_circle.cpp) +target_include_directories(turtle1_circle PRIVATE + $ + $) +target_compile_definitions(turtle1_circle + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(turtle1_circle + "geometry_msgs" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(turtle1_circle PLUGIN "software_training::Turtle1Circle" EXECUTABLE turtle1_circle_node) +install(TARGETS + turtle1_circle + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# ---- spawn_turtle ---- +add_library(spawn_turtle SHARED + src/spawn_turtle.cpp) +target_include_directories(spawn_turtle PRIVATE + $ + $) +target_compile_definitions(spawn_turtle + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(spawn_turtle + "turtlesim" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(spawn_turtle PLUGIN "software_training::SpawnTurtle" EXECUTABLE spawn_turtle_node) +install(TARGETS + spawn_turtle + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# ---- reset_moving_turtle ---- +add_library(reset_moving_turtle SHARED + src/reset_moving_turtle.cpp) +target_include_directories(reset_moving_turtle PRIVATE + $ + $) +target_compile_definitions(reset_moving_turtle + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(reset_moving_turtle + "turtlesim" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(reset_moving_turtle PLUGIN "software_training::ResetMovingTurtle" EXECUTABLE reset_moving_turtle_node) +install(TARGETS + reset_moving_turtle + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +rosidl_target_interfaces(reset_moving_turtle + ${PROJECT_NAME} "rosidl_typesupport_cpp") + + # ---- reset_moving_turtle_client ---- +add_library(reset_moving_turtle_client SHARED + src/reset_moving_turtle_client.cpp) +target_include_directories(reset_moving_turtle_client PRIVATE + $ + $) +target_compile_definitions(reset_moving_turtle_client + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(reset_moving_turtle_client + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(reset_moving_turtle_client PLUGIN "software_training::ResetMovingTurtleClient" EXECUTABLE reset_moving_turtle_client_node) +install(TARGETS + reset_moving_turtle_client + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +rosidl_target_interfaces(reset_moving_turtle_client + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ---- distance_publisher ---- +add_library(distance_publisher SHARED + src/distance_publisher.cpp) +target_include_directories(distance_publisher PRIVATE + $ + $) +target_compile_definitions(distance_publisher + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(distance_publisher + "turtlesim" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(distance_publisher PLUGIN "software_training::DistancePublisher" EXECUTABLE distance_publisher_node) +install(TARGETS + distance_publisher + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +rosidl_target_interfaces(distance_publisher + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ---- distance_subscriber ---- +add_library(distance_subscriber SHARED + src/distance_subscriber.cpp) +target_include_directories(distance_subscriber PRIVATE + $ + $) +target_compile_definitions(distance_subscriber + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(distance_subscriber + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(distance_subscriber PLUGIN "software_training::DistanceSubscriber" EXECUTABLE distance_subscriber_node) +install(TARGETS + distance_subscriber + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +rosidl_target_interfaces(distance_subscriber + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ---- moving_turtle_server ---- +add_library(moving_turtle_server SHARED + src/moving_turtle_server.cpp) +target_include_directories(moving_turtle_server PRIVATE + $ + $) +target_compile_definitions(moving_turtle_server + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(moving_turtle_server + "rclcpp" + "rclcpp_action" + "geometry_msgs" + "turtlesim" + "rclcpp_components") +rclcpp_components_register_node(moving_turtle_server PLUGIN "software_training::MovingTurtleServer" EXECUTABLE moving_turtle_server_node) +install(TARGETS + moving_turtle_server + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +rosidl_target_interfaces(moving_turtle_server + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ---- moving_turtle_client ---- +add_library(moving_turtle_client SHARED + src/moving_turtle_client.cpp) +target_include_directories(moving_turtle_client PRIVATE + $ + $) +target_compile_definitions(moving_turtle_client + PRIVATE "SOFTWARE_TRAINING_CPP_BUILDING_DLL") +ament_target_dependencies(moving_turtle_client + "rclcpp" + "rclcpp_action" + "rclcpp_components") +rclcpp_components_register_node(moving_turtle_client PLUGIN "software_training::MovingTurtleClient" EXECUTABLE moving_turtle_client_node) +install(TARGETS + moving_turtle_client + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +rosidl_target_interfaces(moving_turtle_client + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/software_training/action/MovingTurtle.action b/software_training/action/MovingTurtle.action new file mode 100644 index 0000000..db5b1a2 --- /dev/null +++ b/software_training/action/MovingTurtle.action @@ -0,0 +1,7 @@ +float32 target_x +float32 target_y +--- +uint64 duration +--- +float32 x_distance +float32 y_distance \ No newline at end of file diff --git a/software_training/include/software_training/visibility.h b/software_training/include/software_training/visibility.h new file mode 100644 index 0000000..41d1022 --- /dev/null +++ b/software_training/include/software_training/visibility.h @@ -0,0 +1,44 @@ +#ifndef SOFTWARE_TRAINING__VISIBILITY_CONTROL_H_ +#define SOFTWARE_TRAINING__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define SOFTWARE_TRAINING_EXPORT __attribute__ ((dllexport)) + #define SOFTWARE_TRAINING_IMPORT __attribute__ ((dllimport)) + #else + #define SOFTWARE_TRAINING_EXPORT __declspec(dllexport) + #define SOFTWARE_TRAINING_IMPORT __declspec(dllimport) + #endif + #ifdef SOFTWARE_TRAINING_BUILDING_DLL + #define SOFTWARE_TRAINING_PUBLIC SOFTWARE_TRAINING_EXPORT + #else + #define SOFTWARE_TRAINING_PUBLIC SOFTWARE_TRAINING_IMPORT + #endif + #define SOFTWARE_TRAINING_PUBLIC_TYPE SOFTWARE_TRAINING_PUBLIC + #define SOFTWARE_TRAINING_LOCAL +#else + #define SOFTWARE_TRAINING_EXPORT __attribute__ ((visibility("default"))) + #define SOFTWARE_TRAINING_IMPORT + #if __GNUC__ >= 4 + #define SOFTWARE_TRAINING_PUBLIC __attribute__ ((visibility("default"))) + #define SOFTWARE_TRAINING_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define SOFTWARE_TRAINING_PUBLIC + #define SOFTWARE_TRAINING_LOCAL + #endif + #define SOFTWARE_TRAINING_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // SOFTWARE_TRAINING__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/software_training/launch/launch.py b/software_training/launch/launch.py new file mode 100644 index 0000000..d439db7 --- /dev/null +++ b/software_training/launch/launch.py @@ -0,0 +1,61 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import RegisterEventHandler, LogInfo +from launch.event_handlers import OnProcessStart + +def generate_launch_description(): + + turtlesim = Node( + package='turtlesim', + executable='turtlesim_node', + name='turtle_sim', + ) + + turtle1_circle = Node( + package='software_training', + executable='turtle1_circle_node', + name='turtle1_circle', + ) + + spawn_turtle = Node( + package='software_training', + executable='spawn_turtle_node', + name='spawn_turtle', + ) + + reset_moving_turtle = Node( + package='software_training', + executable='moving_turtle_server_node', + name='moving_turtle_server', + ) + + distance_publisher = Node( + package='software_training', + executable='distance_publisher_node', + name='distance_publisher', + ) + + moving_turtle_server = Node( + package='software_training', + executable='moving_turtle_server_node', + name='moving_turtle_server', + ) + + + launch_after_turtlesim = RegisterEventHandler( + OnProcessStart( + target_action=turtlesim, + on_start=[ + turtle1_circle, + spawn_turtle, + reset_moving_turtle, + distance_publisher, + moving_turtle_server + ] + ) + ) + + return LaunchDescription([ + turtlesim, + launch_after_turtlesim + ]) \ No newline at end of file diff --git a/software_training/msg/Distance.msg b/software_training/msg/Distance.msg new file mode 100644 index 0000000..244c1f4 --- /dev/null +++ b/software_training/msg/Distance.msg @@ -0,0 +1,3 @@ +float32 x_distance +float32 y_distance +float32 distance \ No newline at end of file diff --git a/software_training/package.xml b/software_training/package.xml new file mode 100644 index 0000000..db55af5 --- /dev/null +++ b/software_training/package.xml @@ -0,0 +1,31 @@ + + + + software_training + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + ros2launch + + rclcpp + rclcpp_action + std_srvs + turtlesim + geometry_msgs + rclcpp_components + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/software_training/src/clear_turtle.cpp b/software_training/src/clear_turtle.cpp new file mode 100644 index 0000000..e2d4c23 --- /dev/null +++ b/software_training/src/clear_turtle.cpp @@ -0,0 +1,64 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/empty.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; + +namespace software_training{ + +class ClearTurtle : public rclcpp::Node { + public: + using ClearService = std_srvs::srv::Empty; + + explicit ClearTurtle(const rclcpp::NodeOptions & options) + : Node("clear_turtle_comp", options) { + + this->client = this->create_client("/clear"); + + this->timer = this->create_wall_timer( + std::chrono::milliseconds(300), + std::bind(&ClearTurtle::send_clear_request, this)); + + RCLCPP_INFO(this->get_logger(), "ClearTurtle node started, will attempt to clear turtlesim."); + } + + private: + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + void send_clear_request() { + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available, waiting again..."); + } + + this->timer->cancel(); + auto request = std::make_shared(); + auto result = client->async_send_request(request, + [this](rclcpp::Client::SharedFuture future) { + try + { + future.get(); + RCLCPP_INFO(this->get_logger(), "Turtles cleared!"); + } + catch(const std::exception& e) + { + RCLCPP_INFO(this->get_logger(), "Failed to clear turtles"); + } + rclcpp::shutdown(); + } + ); + } +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::ClearTurtle) diff --git a/software_training/src/distance_publisher.cpp b/software_training/src/distance_publisher.cpp new file mode 100644 index 0000000..0a390d5 --- /dev/null +++ b/software_training/src/distance_publisher.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "turtlesim/msg/pose.hpp" +#include "software_training/msg/distance.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace software_training { + +class DistancePublisher : public rclcpp::Node { + public: + + explicit DistancePublisher(const rclcpp::NodeOptions &options) + : Node("distance_publisher", options) { + + this->moving_turtle_subscriber = this->create_subscription("/moving_turtle/pose", 10, + std::bind(&DistancePublisher::moving_turtle_callback, this, _1)); + this->stationary_turtle_subscriber = this->create_subscription("/stationary_turtle/pose", 10, + std::bind(&DistancePublisher::stationary_turtle_callback, this, _1)); + + this->publisher = this->create_publisher("/moving_to_stationary_distance", 10); + this->timer = this->create_wall_timer(1s, std::bind(&DistancePublisher::publish_distance, this)); + } + + private: + using Pose = turtlesim::msg::Pose; + using Distance = software_training::msg::Distance; + + rclcpp::TimerBase::SharedPtr timer; + + rclcpp::Subscription::SharedPtr moving_turtle_subscriber; + rclcpp::Subscription::SharedPtr stationary_turtle_subscriber; + rclcpp::Publisher::SharedPtr publisher; + + Pose::SharedPtr moving_turtle_pose; + Pose::SharedPtr stationary_turtle_pose; + + void moving_turtle_callback(const Pose::SharedPtr pose) { + moving_turtle_pose = pose; + } + void stationary_turtle_callback(const Pose::SharedPtr pose) { + stationary_turtle_pose = pose; + } + + void publish_distance() { + auto message = Distance(); + message.x_distance = fabs(moving_turtle_pose->x - stationary_turtle_pose->x); + message.y_distance = fabs(moving_turtle_pose->y - stationary_turtle_pose->y); + message.distance = sqrt(message.x_distance*message.x_distance + message.y_distance*message.y_distance); + + RCLCPP_INFO(this->get_logger(), "Publishing: x: %.2f, y: %.2f, distance: %.2f", message.x_distance, message.y_distance, message.distance); + publisher->publish(message); + } +}; +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::DistancePublisher) diff --git a/software_training/src/distance_subscriber.cpp b/software_training/src/distance_subscriber.cpp new file mode 100644 index 0000000..a769a7d --- /dev/null +++ b/software_training/src/distance_subscriber.cpp @@ -0,0 +1,32 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "software_training/msg/distance.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::placeholders; + +namespace software_training { + +class DistanceSubscriber : public rclcpp::Node { + public: + + explicit DistanceSubscriber(const rclcpp::NodeOptions &options) + : Node("distance_subscriber", options) { + + this->subscribtion = this->create_subscription("/moving_to_stationary_distance", 10, + std::bind(&DistanceSubscriber::subscription_callback, this, _1)); + } + + private: + using Distance = software_training::msg::Distance; + rclcpp::Subscription::SharedPtr subscribtion; + + void subscription_callback(const Distance::SharedPtr message) { + RCLCPP_INFO(this->get_logger(), "Listened: x: %.2f, y: %.2f, distance: %.2f", message->x_distance, message->y_distance, message->distance); + } +}; +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::DistanceSubscriber) diff --git a/software_training/src/moving_turtle_client.cpp b/software_training/src/moving_turtle_client.cpp new file mode 100644 index 0000000..8fbeb84 --- /dev/null +++ b/software_training/src/moving_turtle_client.cpp @@ -0,0 +1,99 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "software_training/action/moving_turtle.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace software_training { +class MovingTurtleClient : public rclcpp::Node { + public: + explicit MovingTurtleClient(const rclcpp::NodeOptions &options) + : Node("moving_turtle_client", options) { + + this->declare_parameter("x", 3.0); + this->declare_parameter("y", 4.0); + + target[0] = (float)this->get_parameter("x").as_double(); + target[1] = (float)this->get_parameter("y").as_double(); + + this->client = rclcpp_action::create_client(this, "moving_turtle"); + + this->timer = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&MovingTurtleClient::send_goal, this)); + + } + + private: + using MovingTurtle = software_training::action::MovingTurtle; + using GoalHandleMovingTurtle = rclcpp_action::ClientGoalHandle; + + rclcpp_action::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + float target[2]; + + void send_goal() { + this->timer->cancel(); + if (!this->client->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = MovingTurtle::Goal(); + goal_msg.target_x = target[0]; + goal_msg.target_y = target[1]; + + RCLCPP_INFO(this->get_logger(), "Sending goal x: %.2f, y: %.2f", goal_msg.target_x, goal_msg.target_y); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&MovingTurtleClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&MovingTurtleClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&MovingTurtleClient::result_callback, this, _1); + this->client->async_send_goal(goal_msg, send_goal_options); + } + + void goal_response_callback(std::shared_future future) { + auto goal_handle = future.get(); + 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 feedback_callback(GoalHandleMovingTurtle::SharedPtr, const std::shared_ptr feedback) { + RCLCPP_INFO(this->get_logger(), "Feedback distance x: %.2f, y: %.2f", feedback->x_distance, feedback->y_distance); + } + + void result_callback(const GoalHandleMovingTurtle::WrappedResult & result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + RCLCPP_INFO(this->get_logger(), "Result received in %lu nanoseconds", result.result->duration); + rclcpp::shutdown(); + } +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::MovingTurtleClient) diff --git a/software_training/src/moving_turtle_server.cpp b/software_training/src/moving_turtle_server.cpp new file mode 100644 index 0000000..533c94d --- /dev/null +++ b/software_training/src/moving_turtle_server.cpp @@ -0,0 +1,155 @@ +#include +#include +#include +#include +#include + +#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/moving_turtle.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#define kp 2 + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace software_training { + +double normalize_angle(float angle) { + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +} + +class MovingTurtleServer : public rclcpp::Node { + public: + explicit MovingTurtleServer(const rclcpp::NodeOptions &options) + : Node("moving_turtle_server", options) { + + this->twist_publisher = this->create_publisher("/moving_turtle/cmd_vel", 10); + this->pose_subscriber = this->create_subscription("/moving_turtle/pose", 10, + std::bind(&MovingTurtleServer::moving_turtle_pose_callback, this, _1)); + this->action_server = rclcpp_action::create_server( + this, + "moving_turtle", + std::bind(&MovingTurtleServer::handle_goal, this, _1, _2), + std::bind(&MovingTurtleServer::handle_cancel, this, _1), + std::bind(&MovingTurtleServer::handle_accepted, this, _1)); + + RCLCPP_INFO(this->get_logger(), "MovingTurtleServer started successfully and is waiting for request!"); + } + + private: + using Twist = geometry_msgs::msg::Twist; + using Pose = turtlesim::msg::Pose; + using MovingTurtle = software_training::action::MovingTurtle; + using GoalHandleMovingTurtle = rclcpp_action::ServerGoalHandle; + + rclcpp::Publisher::SharedPtr twist_publisher; + rclcpp::Subscription::SharedPtr pose_subscriber; + rclcpp_action::Server::SharedPtr action_server; + + Pose::SharedPtr moving_turtle_pose; + + void moving_turtle_pose_callback(const Pose::SharedPtr pose) { + moving_turtle_pose = pose; + } + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + + RCLCPP_INFO(this->get_logger(), "Received goal request with x: %.2f and y: %.2f", goal->target_x, goal->target_y); + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle) { + (void)goal_handle; + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) { + std::thread{std::bind(&MovingTurtleServer::execute, this, _1), goal_handle}.detach(); + } + + void execute(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Time start = this->now(); + rclcpp::Rate rate{5}; + + auto result = std::make_shared(); + + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + + bool rotate_goal = false; + + while(rclcpp::ok()) { + if (goal_handle->is_canceling()) { + rclcpp::Time end = this->now(); + rclcpp::Duration timer_length = end - start; + result->duration = timer_length.nanoseconds(); + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + + float delta_x = goal->target_x - moving_turtle_pose->x; + float delta_y = goal->target_y - moving_turtle_pose->y; + + float distance = std::hypot(delta_x, delta_y); + + // calculate desired angle turtle should be at to reach goal in a straight line + float ref_angle = normalize_angle(atan2(delta_y, delta_x)); + float theta_error = ref_angle - moving_turtle_pose->theta; + + auto twist_msg = Twist(); + + if(distance < 0.05) { + twist_msg.linear.x = 0.0; + twist_msg.angular.z = 0.0; + twist_publisher->publish(twist_msg); + rclcpp::Time end = this->now(); + rclcpp::Duration timer_length = end - start; + result->duration = timer_length.nanoseconds(); + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + return; + } + + if(fabs(theta_error) > 0.01) { + twist_msg.angular.z = kp * theta_error; + } + else { + twist_msg.angular.z = 0; + rotate_goal = true; + } + + if(rotate_goal) { + twist_msg.linear.x = kp * distance; + } + + twist_publisher->publish(twist_msg); + + feedback->x_distance = delta_x; + feedback->y_distance = delta_y; + + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Published feedback x: %.2f, y: %.2f", feedback->x_distance, feedback->y_distance); + + rate.sleep(); + } + } +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::MovingTurtleServer) diff --git a/software_training/src/reset_moving_turtle.cpp b/software_training/src/reset_moving_turtle.cpp new file mode 100644 index 0000000..09a8799 --- /dev/null +++ b/software_training/src/reset_moving_turtle.cpp @@ -0,0 +1,72 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "software_training/srv/reset.hpp" +#include "turtlesim/srv/teleport_absolute.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace software_training { + +class ResetMovingTurtle : public rclcpp::Node { + public: + using Reset = software_training::srv::Reset; + using Teleport = turtlesim::srv::TeleportAbsolute; + + explicit ResetMovingTurtle(const rclcpp::NodeOptions &options) + : Node("clear_turtle_comp", options) { + + this->client = this->create_client("/moving_turtle/teleport_absolute"); + this->service = this->create_service("/reset_moving_turtle", + std::bind(&ResetMovingTurtle::reset_callback, this, _1, _2)); + + RCLCPP_INFO(this->get_logger(), "Waiting for client request."); + } + + private: + rclcpp::Client::SharedPtr client; + rclcpp::Service::SharedPtr service; + + void reset_callback(const std::shared_ptr request, + std::shared_ptr response) { + + (void)request; + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available, waiting again..."); + } + + auto tp_request = std::make_shared(); + + tp_request->x = 25; + tp_request->y = 10; + tp_request->theta = 0; + + auto result = client->async_send_request(tp_request, + [this, response](rclcpp::Client::SharedFuture future) { + try + { + future.get(); + RCLCPP_INFO(this->get_logger(), "Reset moving_turtle!"); + } + catch(const std::exception& e) + { + RCLCPP_INFO(this->get_logger(), "Failed to reset moving_turtle"); + } + } + ); + response->success = true; + } +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::ResetMovingTurtle) diff --git a/software_training/src/reset_moving_turtle_client.cpp b/software_training/src/reset_moving_turtle_client.cpp new file mode 100644 index 0000000..947f88f --- /dev/null +++ b/software_training/src/reset_moving_turtle_client.cpp @@ -0,0 +1,64 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "software_training/srv/reset.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace software_training{ + +class ResetMovingTurtleClient : public rclcpp::Node { + public: + using Reset = software_training::srv::Reset; + + explicit ResetMovingTurtleClient(const rclcpp::NodeOptions & options) + : Node("reset_moving_turtle_client", options) { + + this->client = this->create_client("/reset_moving_turtle"); + + this->timer = this->create_wall_timer( + std::chrono::milliseconds(300), + std::bind(&ResetMovingTurtleClient::send_reset_request, this)); + + RCLCPP_INFO(this->get_logger(), "ResetMovingTurtleClient node started."); + } + + private: + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + void send_reset_request() { + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available, waiting again..."); + } + + this->timer->cancel(); + auto request = std::make_shared(); + auto result = client->async_send_request(request, + [this](rclcpp::Client::SharedFuture future) { + try + { + RCLCPP_INFO(this->get_logger(), "Turtle reset: %s", future.get()->success ? "true" : "false"); + } + catch(const std::exception& e) + { + RCLCPP_INFO(this->get_logger(), "Failed to reset turtles"); + } + rclcpp::shutdown(); + } + ); + } +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::ResetMovingTurtleClient) diff --git a/software_training/src/spawn_turtle.cpp b/software_training/src/spawn_turtle.cpp new file mode 100644 index 0000000..bc60400 --- /dev/null +++ b/software_training/src/spawn_turtle.cpp @@ -0,0 +1,90 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "turtlesim/srv/spawn.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; + +namespace software_training{ + +class SpawnTurtle : public rclcpp::Node { + public: + explicit SpawnTurtle(const rclcpp::NodeOptions & options) + : Node("spawn_turtle_comp", options) { + + this->client = this->create_client("/spawn"); + + this->timer = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&SpawnTurtle::send_spawn_request, this)); + + RCLCPP_INFO(this->get_logger(), "SpawnTurtle node started, will attempt to spawn turtles."); + } + + private: + using SpawnService = turtlesim::srv::Spawn; + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + struct turtle_info { + std::string name; + float x; + float y; + float theta; + }; + + turtle_info turtles_to_spawn[2] = { + {"stationary_turtle", 5, 5, 0}, + {"moving_turtle", 7, 8, 0} + }; + + void send_spawn_request() { + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available, waiting again..."); + } + + this->timer->cancel(); + + for (turtle_info turtle : turtles_to_spawn) { + auto request = std::make_shared(); + + request->name = turtle.name; + request->x = turtle.x; + request->y = turtle.y; + request->theta = turtle.theta; + + std::string requested_turtle = turtle.name; + + auto result = client->async_send_request(request, + [this, requested_turtle](rclcpp::Client::SharedFuture future) { + try + { + future.get(); + RCLCPP_INFO(this->get_logger(), "Spawned %s", requested_turtle.c_str()); + } + catch(const std::exception& e) + { + RCLCPP_INFO(this->get_logger(), "Failed to spawn %s", requested_turtle.c_str()); + } + + if (requested_turtle == "moving_turtle") { + rclcpp::shutdown(); + } + } + ); + } + + } +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::SpawnTurtle) diff --git a/software_training/src/turtle1_circle.cpp b/software_training/src/turtle1_circle.cpp new file mode 100644 index 0000000..b913ccf --- /dev/null +++ b/software_training/src/turtle1_circle.cpp @@ -0,0 +1,48 @@ +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; + +namespace software_training { + +class Turtle1Circle : public rclcpp::Node { + + public: + + explicit Turtle1Circle(const rclcpp::NodeOptions & options) + : Node("turtle1_circle", options) { + + this->publisher = this->create_publisher("/turtle1/cmd_vel", 10); + + this->timer = this->create_wall_timer(1ms, + [this](void) { + auto message = std::make_unique(); + message->linear.x = 2; + message->linear.y = 0; + message->linear.z = 0; + + message->angular.x = 0; + message->angular.y = 0; + message->angular.z = 2; + + RCLCPP_INFO(this->get_logger(), "Publishing message."); + + publisher->publish(std::move(message)); + } + ); + } + + private: + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr publisher; + +}; + +} // namespace software_training + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::Turtle1Circle) \ No newline at end of file diff --git a/software_training/srv/Reset.srv b/software_training/srv/Reset.srv new file mode 100644 index 0000000..e2c59ee --- /dev/null +++ b/software_training/srv/Reset.srv @@ -0,0 +1,3 @@ + +--- +bool success \ No newline at end of file