diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..600d2d3 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode \ No newline at end of file diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt new file mode 100644 index 0000000..bb7955c --- /dev/null +++ b/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;$\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;$\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;$\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;$\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;$\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;$\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() diff --git a/software_training/action/Waypoint.action b/software_training/action/Waypoint.action new file mode 100644 index 0000000..0cd487c --- /dev/null +++ b/software_training/action/Waypoint.action @@ -0,0 +1,6 @@ +float32 waypoint_x +float32 waypoint_y +--- +float32 time_s +--- +float32 distance_to_waypoint \ No newline at end of file diff --git a/software_training/include/software_training/clear_turtles_server.hpp b/software_training/include/software_training/clear_turtles_server.hpp new file mode 100644 index 0000000..cfc5049 --- /dev/null +++ b/software_training/include/software_training/clear_turtles_server.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#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 TURTLE_NAMES = {"turtle1", "moving_turtle", "stationary_turtle"}; + + rclcpp::Service::SharedPtr service; + rclcpp::Client::SharedPtr kill_turtles_client; + + // Service to clear all existing turtles + SOFTWARE_TRAINING_LOCAL + void clear_turtles(const std::shared_ptr request, + std::shared_ptr response); +}; +} diff --git a/software_training/include/software_training/move_to_waypoint_action_server.hpp b/software_training/include/software_training/move_to_waypoint_action_server.hpp new file mode 100644 index 0000000..6a6a3e7 --- /dev/null +++ b/software_training/include/software_training/move_to_waypoint_action_server.hpp @@ -0,0 +1,62 @@ +#pragma once + +#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/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; + + 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::SharedPtr server; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::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 goal); + + SOFTWARE_TRAINING_LOCAL + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + + SOFTWARE_TRAINING_LOCAL + void handle_accepted(const std::shared_ptr goal_handle); + + SOFTWARE_TRAINING_LOCAL + void execute(const std::shared_ptr goal_handle); + + SOFTWARE_TRAINING_LOCAL + float distance_to_point(float x, float y); +}; + +} diff --git a/software_training/include/software_training/move_turtle1_in_circle_publisher.hpp b/software_training/include/software_training/move_turtle1_in_circle_publisher.hpp new file mode 100644 index 0000000..3fa34f8 --- /dev/null +++ b/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::SharedPtr publisher; + + SOFTWARE_TRAINING_LOCAL + void publish(void); +}; +} diff --git a/software_training/include/software_training/reset_moving_turtle_server.hpp b/software_training/include/software_training/reset_moving_turtle_server.hpp new file mode 100644 index 0000000..a301dc4 --- /dev/null +++ b/software_training/include/software_training/reset_moving_turtle_server.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +#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::SharedPtr service; + rclcpp::Client::SharedPtr teleport_turtle_client; + + // Service to reset moving_turtle to its starting position + SOFTWARE_TRAINING_LOCAL + void reset_moving_turtle(const std::shared_ptr request, + std::shared_ptr response); +}; +} diff --git a/software_training/include/software_training/spawn_turtles_server.hpp b/software_training/include/software_training/spawn_turtles_server.hpp new file mode 100644 index 0000000..2f11205 --- /dev/null +++ b/software_training/include/software_training/spawn_turtles_server.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include + +#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_descriptions = { + {"stationary_turtle", 5, 5}, + {"moving_turtle", 25, 10} + }; + + rclcpp::Service::SharedPtr service; + rclcpp::Client::SharedPtr spawn_turtle_client; + + // Service to spawn stationary_turtle and moving_turtle + SOFTWARE_TRAINING_LOCAL + void spawn_turtles(const std::shared_ptr request, + std::shared_ptr response); +}; +} diff --git a/software_training/include/software_training/turtle_distance_publisher.hpp b/software_training/include/software_training/turtle_distance_publisher.hpp new file mode 100644 index 0000000..02fe562 --- /dev/null +++ b/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::SharedPtr publisher; + rclcpp::Subscription::SharedPtr stationary_turtle_subscription; + rclcpp::Subscription::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); +}; +} diff --git a/software_training/include/software_training/visibility_control.h b/software_training/include/software_training/visibility_control.h new file mode 100644 index 0000000..1d37d8d --- /dev/null +++ b/software_training/include/software_training/visibility_control.h @@ -0,0 +1,41 @@ +#pragma once + +#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 diff --git a/software_training/launch/software_training.launch.py b/software_training/launch/software_training.launch.py new file mode 100644 index 0000000..0f5453b --- /dev/null +++ b/software_training/launch/software_training.launch.py @@ -0,0 +1,66 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + """Generate launch description with multiple components.""" + return LaunchDescription([ + Node( + package='turtlesim', + executable='turtlesim_node', + name='turtlesim_node' + ), + ComposableNodeContainer( + name='container1', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='software_training', + plugin='software_training::clear_turtles_server', + name='clear_turtles_server'), + ComposableNode( + package='software_training', + plugin='software_training::spawn_turtles_server', + name='spawn_turtles_server'), + ComposableNode( + package='software_training', + plugin='software_training::reset_moving_turtle_server', + name='reset_moving_turtle_server'), + ComposableNode( + package='software_training', + plugin='software_training::move_to_waypoint_action_server', + name='move_to_waypoint_action_server') + ], + output='screen', + ), + ComposableNodeContainer( + name='container2', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='software_training', + plugin='software_training::move_turtle1_in_circle_publisher', + name='move_turtle1_in_circle_publisher') + ], + output='screen', + ), + ComposableNodeContainer( + name='container3', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='software_training', + plugin='software_training::turtle_distance_publisher', + name='turtle_distance_publisher') + ], + output='screen', + ) + ]) diff --git a/software_training/msg/Distance.msg b/software_training/msg/Distance.msg new file mode 100644 index 0000000..f3ef9b4 --- /dev/null +++ b/software_training/msg/Distance.msg @@ -0,0 +1,3 @@ +float32 delta_x +float32 delta_y +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..43860ef --- /dev/null +++ b/software_training/package.xml @@ -0,0 +1,31 @@ + + + + software_training + 0.0.0 + UWRT Software Training + Cindy Li + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + geometry_msgs + turtlesim + action_msgs + + rosidl_default_generators + rclcpp_components + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/software_training/src/clear_turtles_server.cpp b/software_training/src/clear_turtles_server.cpp new file mode 100644 index 0000000..9df77e0 --- /dev/null +++ b/software_training/src/clear_turtles_server.cpp @@ -0,0 +1,41 @@ +#include "software_training/clear_turtles_server.hpp" + +namespace software_training { + +clear_turtles_server::clear_turtles_server(const rclcpp::NodeOptions& options) : rclcpp::Node("clear_turtles_server", options) { + service = this->create_service("clear_turtles", + std::bind(&clear_turtles_server::clear_turtles, this, std::placeholders::_1, std::placeholders::_2)); + kill_turtles_client = this->create_client("/kill"); +} + +void clear_turtles_server::clear_turtles(const std::shared_ptr request, + std::shared_ptr response) { + (void)request; + + if(!kill_turtles_client->wait_for_service(2s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + response->success = false; + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Service not available after trying for 2s. Exiting."); + response->success = false; + return; + } + + for (auto& turtle_name: TURTLE_NAMES) { + auto kill_request = std::make_shared(); + kill_request->name = turtle_name; + + auto result = kill_turtles_client->async_send_request(kill_request); + } + + response->success = true; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "All turtles cleared"); +} + +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::clear_turtles_server) \ No newline at end of file diff --git a/software_training/src/move_to_waypoint_action_server.cpp b/software_training/src/move_to_waypoint_action_server.cpp new file mode 100644 index 0000000..250afc6 --- /dev/null +++ b/software_training/src/move_to_waypoint_action_server.cpp @@ -0,0 +1,111 @@ +#include "software_training/move_to_waypoint_action_server.hpp" + +namespace software_training { + +move_to_waypoint_action_server::move_to_waypoint_action_server(const rclcpp::NodeOptions& options) : rclcpp::Node("move_to_waypoint_action_server", options) { + using namespace std::placeholders; + this->server = rclcpp_action::create_server( + this, + "move_to_waypoint", + std::bind(&move_to_waypoint_action_server::handle_goal, this, _1, _2), + std::bind(&move_to_waypoint_action_server::handle_cancel, this, _1), + std::bind(&move_to_waypoint_action_server::handle_accepted, this, _1) + ); + + publisher = this->create_publisher("/moving_turtle/cmd_vel", 10); + + moving_turtle_subscription = this->create_subscription( + "/moving_turtle/pose", + 10, + std::bind(&move_to_waypoint_action_server::subscription_callback, this, _1) + ); +} + +void move_to_waypoint_action_server::subscription_callback(const turtlesim::msg::Pose::SharedPtr msg) { + moving_turtle_pose.x = msg->x; + moving_turtle_pose.y = msg->y; + moving_turtle_pose.theta = msg->theta; +} + +rclcpp_action::GoalResponse move_to_waypoint_action_server::handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Received request to move to waypoint (%.3f, %3f)", goal->waypoint_x, goal->waypoint_y); + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse move_to_waypoint_action_server::handle_cancel(const std::shared_ptr goal_handle) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void move_to_waypoint_action_server::handle_accepted(const std::shared_ptr goal_handle) { + using namespace std::placeholders; + std::thread{std::bind(&move_to_waypoint_action_server::execute, this, _1), goal_handle}.detach(); +} + +void move_to_waypoint_action_server::execute(const std::shared_ptr goal_handle) { + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + float time_s = 0; + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Moving moving_turtle to waypoint (%.3f, %.3f)", goal->waypoint_x, goal->waypoint_y); + rclcpp::Rate loop_rate(LOOP_RATE_HZ); + + while (rclcpp::ok() && distance_to_point(goal->waypoint_x, goal->waypoint_y) > FLOAT_TOLERANCE) { + // check if cancelling goal + if (goal_handle->is_canceling()) { + result->time_s = time_s; + goal_handle->canceled(result); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Goal cancelled"); + return; + } + + // move towards waypoint: first rotate to face waypoint, then move straight + // this is a really dumb and simple way of doing this, but it mostly works lol + float angle_to_waypoint = atan2(goal->waypoint_y - moving_turtle_pose.y, goal->waypoint_x - moving_turtle_pose.x); + if (std::fabs(angle_to_waypoint - moving_turtle_pose.theta) > 0.2) { + // rotate + auto msg = geometry_msgs::msg::Twist(); + msg.linear.x = 0; + msg.linear.y = 0; + msg.angular.z = 2; + publisher->publish(msg); + } + else { + // move straight + auto msg = geometry_msgs::msg::Twist(); + msg.linear.x = 2; + msg.linear.y = 0; + msg.angular.z = 0; + publisher->publish(msg); + } + + // publish feedback + float distance = distance_to_point(goal->waypoint_x, goal->waypoint_y); + feedback->distance_to_waypoint = distance; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Distance to waypoint: %.3f", distance); + + time_s += 1/LOOP_RATE_HZ; + loop_rate.sleep(); + } + + // check if waypoint is reached + if (rclcpp::ok()) { + result->time_s = time_s; + goal_handle->succeed(result); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Waypoint reached in %.3fs", time_s); + } +} + +float move_to_waypoint_action_server::distance_to_point(float x, float y) { + return std::sqrt(std::pow(x - moving_turtle_pose.x, 2) + std::pow(y - moving_turtle_pose.y, 2)); +} + +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::move_to_waypoint_action_server) diff --git a/software_training/src/move_turtle1_in_circle_publisher.cpp b/software_training/src/move_turtle1_in_circle_publisher.cpp new file mode 100644 index 0000000..3089bad --- /dev/null +++ b/software_training/src/move_turtle1_in_circle_publisher.cpp @@ -0,0 +1,21 @@ +#include "software_training/move_turtle1_in_circle_publisher.hpp" + +namespace software_training { + +move_turtle1_in_circle_publisher::move_turtle1_in_circle_publisher(const rclcpp::NodeOptions& options) : rclcpp::Node("move_turtle1_in_circle_publisher", options) { + publisher = this->create_publisher("/turtle1/cmd_vel", 10); + timer = this->create_wall_timer(10ms, std::bind(&move_turtle1_in_circle_publisher::publish, this)); +} + +void move_turtle1_in_circle_publisher::publish(void) { + auto msg = geometry_msgs::msg::Twist(); + msg.linear.x = 1; + msg.angular.z = 1; + publisher->publish(msg); +} + +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::move_turtle1_in_circle_publisher) diff --git a/software_training/src/reset_moving_turtle_server.cpp b/software_training/src/reset_moving_turtle_server.cpp new file mode 100644 index 0000000..f7cea54 --- /dev/null +++ b/software_training/src/reset_moving_turtle_server.cpp @@ -0,0 +1,41 @@ +#include "software_training/reset_moving_turtle_server.hpp" + +namespace software_training { + +reset_moving_turtle_server::reset_moving_turtle_server(const rclcpp::NodeOptions& options) : rclcpp::Node("reset_moving_turtle_server", options) { + service = this->create_service("reset_moving_turtle", + std::bind(&reset_moving_turtle_server::reset_moving_turtle, this, std::placeholders::_1, std::placeholders::_2)); + teleport_turtle_client = this->create_client("/moving_turtle/teleport_absolute"); +} + +void reset_moving_turtle_server::reset_moving_turtle(const std::shared_ptr request, + std::shared_ptr response) { + (void)request; + + if(!teleport_turtle_client->wait_for_service(2s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + response->success = false; + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Service not available after trying for 2s. Exiting."); + response->success = false; + return; + } + + auto teleport_request = std::make_shared(); + teleport_request->x = START_X; + teleport_request->y = START_Y; + teleport_request->theta = 0; + + auto result = teleport_turtle_client->async_send_request(teleport_request); + + response->success = true; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "moving_turtle reset"); +} + +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::reset_moving_turtle_server) \ No newline at end of file diff --git a/software_training/src/spawn_turtles_server.cpp b/software_training/src/spawn_turtles_server.cpp new file mode 100644 index 0000000..b8dddc0 --- /dev/null +++ b/software_training/src/spawn_turtles_server.cpp @@ -0,0 +1,44 @@ +#include "software_training/spawn_turtles_server.hpp" + +namespace software_training { + +spawn_turtles_server::spawn_turtles_server(const rclcpp::NodeOptions& options) : rclcpp::Node("spawn_turtles_server", options) { + service = this->create_service("spawn_turtles", + std::bind(&spawn_turtles_server::spawn_turtles, this, std::placeholders::_1, std::placeholders::_2)); + spawn_turtle_client = this->create_client("/spawn"); +} + +void spawn_turtles_server::spawn_turtles(const std::shared_ptr request, + std::shared_ptr response) { + (void)request; + + if(!spawn_turtle_client->wait_for_service(2s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + response->success = false; + return; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Service not available after trying for 2s. Exiting."); + response->success = false; + return; + } + + for (turtle_descriptor& turtle: turtle_descriptions) { + auto spawn_request = std::make_unique(); + spawn_request->x = turtle.x; + spawn_request->y = turtle.y; + spawn_request->theta = 0; + spawn_request->name = turtle.name; + + auto result = spawn_turtle_client->async_send_request(std::move(spawn_request)); + } + + response->success = true; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "All turtles spawned"); +} + +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::spawn_turtles_server) \ No newline at end of file diff --git a/software_training/src/turtle_distance_publisher.cpp b/software_training/src/turtle_distance_publisher.cpp new file mode 100644 index 0000000..10c4339 --- /dev/null +++ b/software_training/src/turtle_distance_publisher.cpp @@ -0,0 +1,44 @@ +#include "software_training/turtle_distance_publisher.hpp" + +namespace software_training { + +turtle_distance_publisher::turtle_distance_publisher(const rclcpp::NodeOptions& options) : rclcpp::Node("turtle_distance_publisher", options) { + publisher = this->create_publisher("/turtle_distance", 10); + timer = this->create_wall_timer(1000ms, std::bind(&turtle_distance_publisher::publish, this)); + + stationary_turtle_subscription = this->create_subscription( + "/stationary_turtle/pose", + 10, + std::bind(&turtle_distance_publisher::stationary_turtle_callback, this, std::placeholders::_1) + ); + + moving_turtle_subscription = this->create_subscription( + "/moving_turtle/pose", + 10, + std::bind(&turtle_distance_publisher::moving_turtle_callback, this, std::placeholders::_1) + ); +} + +void turtle_distance_publisher::publish(void) { + auto msg = software_training::msg::Distance(); + msg.delta_x = std::fabs(stationary_turtle_position.x - moving_turtle_position.x); + msg.delta_y = std::fabs(stationary_turtle_position.y - moving_turtle_position.y); + msg.distance = std::sqrt(std::pow(msg.delta_x, 2) + std::pow(msg.delta_y, 2)); + publisher->publish(msg); +} + +void turtle_distance_publisher::stationary_turtle_callback(const turtlesim::msg::Pose::SharedPtr msg) { + stationary_turtle_position.x = msg->x; + stationary_turtle_position.y = msg->y; +} + +void turtle_distance_publisher::moving_turtle_callback(const turtlesim::msg::Pose::SharedPtr msg) { + moving_turtle_position.x = msg->x; + moving_turtle_position.y = msg->y; +} + +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(software_training::turtle_distance_publisher) diff --git a/software_training/srv/Success.srv b/software_training/srv/Success.srv new file mode 100644 index 0000000..1308cc2 --- /dev/null +++ b/software_training/srv/Success.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file