diff --git a/software_training_assignment/.gitignore b/software_training_assignment/.gitignore new file mode 100644 index 0000000..6bef416 --- /dev/null +++ b/software_training_assignment/.gitignore @@ -0,0 +1,2 @@ +.vscode/ +software_training_assignment/launch/__pycache__/ diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt new file mode 100644 index 0000000..004befb --- /dev/null +++ b/software_training_assignment/CMakeLists.txt @@ -0,0 +1,164 @@ +cmake_minimum_required(VERSION 3.5) +project(software_training_assignment) + +# 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() + +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() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(turtlesim REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(rclcpp_action REQUIRED) + +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +include_directories(include) + +set(node_plugins "") + +# custom services and messages and actions +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/ExampleService.srv" + "msg/Position.msg" + "action/MoveTo.action" + DEPENDENCIES std_msgs geometry_msgs builtin_interfaces) +ament_export_dependencies(rosidl_default_runtime) + +#spawn turtles +add_library(spawn_turtle SHARED + src/spawn_turtle.cpp) +target_include_directories(spawn_turtle PRIVATE + $ + $) +target_compile_definitions(spawn_turtle + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(spawn_turtle + "turtlesim" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(spawn_turtle PLUGIN "composition::SpawnTurtle" EXECUTABLE spawn) + +#clear turtles +add_library(clear SHARED + src/clear_turtle.cpp) +target_include_directories(clear PRIVATE + $ + $) +target_compile_definitions(clear + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(clear + "turtlesim" + "rclcpp" + "rclcpp_components") +rclcpp_components_register_node(clear PLUGIN "composition::ClearTurtle" EXECUTABLE clear_turtle) + +#rotate turtle 1 +add_library(rotate SHARED + src/rotate_turtle1.cpp) +target_include_directories(rotate PRIVATE + $ + $) +target_compile_definitions(rotate + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(rotate + "turtlesim" + "rclcpp" + "rclcpp_components" + "geometry_msgs") +rclcpp_components_register_node(rotate PLUGIN "composition::RotateTurtle" EXECUTABLE rotate_turtle) + +#reset turtle service +add_library(reset SHARED + src/reset_moving.cpp) +target_include_directories(reset PRIVATE + $ + $) +target_compile_definitions(reset + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(reset + "turtlesim" + "rclcpp" + "rclcpp_components") +rosidl_target_interfaces(reset ${PROJECT_NAME} "rosidl_typesupport_cpp") +rclcpp_components_register_node(reset PLUGIN "composition::ResetMoving" EXECUTABLE reset_turtle) + +#stationary position publisher +add_library(pos_pub SHARED + src/stationary_position_publisher.cpp) +target_include_directories(pos_pub PRIVATE + $ + $) +target_compile_definitions(pos_pub + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(pos_pub + "turtlesim" + "rclcpp" + "rclcpp_components") +rosidl_target_interfaces(pos_pub ${PROJECT_NAME} "rosidl_typesupport_cpp") +rclcpp_components_register_node(pos_pub PLUGIN "composition::StationaryPosPublisher" EXECUTABLE stationary_publisher) + +#move turtle server +add_library(move_server SHARED + src/move_turtle_server.cpp) +target_include_directories(move_server PRIVATE + $ + $) +target_compile_definitions(move_server + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(move_server + "turtlesim" + "rclcpp" + "rclcpp_components" + "rclcpp_action") +rosidl_target_interfaces(move_server ${PROJECT_NAME} "rosidl_typesupport_cpp") +rclcpp_components_register_node(move_server PLUGIN "composition::MoveTurtleServer" EXECUTABLE move_turtle) + + +install(TARGETS + spawn_turtle + clear + rotate + reset + pos_pub + move_server + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + + +ament_package() + + + + diff --git a/software_training_assignment/action/MoveTo.action b/software_training_assignment/action/MoveTo.action new file mode 100644 index 0000000..4efa196 --- /dev/null +++ b/software_training_assignment/action/MoveTo.action @@ -0,0 +1,5 @@ +float32[] position +--- +int32 seconds +--- +float32 distance diff --git a/software_training_assignment/include/clear_turtle.hpp b/software_training_assignment/include/clear_turtle.hpp new file mode 100644 index 0000000..c8b8660 --- /dev/null +++ b/software_training_assignment/include/clear_turtle.hpp @@ -0,0 +1,28 @@ +#ifndef CLEAR_H +#define CLEAR_H + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include +#include + +namespace composition +{ +class ClearTurtle : public rclcpp::Node +{ + public: + SOFTWARE_TRAINING_PUBLIC + explicit ClearTurtle(const rclcpp::NodeOptions & options); + + private: + SOFTWARE_TRAINING_LOCAL + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + void kill_turtles(); + +}; +} + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/move_turtle_server.hpp b/software_training_assignment/include/move_turtle_server.hpp new file mode 100644 index 0000000..ba07ec1 --- /dev/null +++ b/software_training_assignment/include/move_turtle_server.hpp @@ -0,0 +1,42 @@ +#ifndef MOVE_H +#define MOVE_H + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include +#include + +#include + +namespace composition +{ +class MoveTurtleServer : public rclcpp::Node +{ + public: + using MoveTo = software_training_assignment::action::MoveTo; + using GoalHandleMoveTo = rclcpp_action::ServerGoalHandle; + + MoveTurtleServer(const rclcpp::NodeOptions & options); + + private: + rclcpp_action::Server::SharedPtr action_server; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr pose_sub; + + float curr_x; + float curr_y; + std::vector target; + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + void get_pose(const turtlesim::msg::Pose::SharedPtr currentPose); +}; +} + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/reset_moving.hpp b/software_training_assignment/include/reset_moving.hpp new file mode 100644 index 0000000..cc90e61 --- /dev/null +++ b/software_training_assignment/include/reset_moving.hpp @@ -0,0 +1,31 @@ +#ifndef RESET_H +#define RESET_H + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include +#include +#include + +#include +#include + +namespace composition +{ +class ResetMoving : public rclcpp::Node +{ + public: + explicit ResetMoving(const rclcpp::NodeOptions & options); + + private: + rclcpp::Client::SharedPtr teleport_cli; + rclcpp::Service::SharedPtr teleport_srv; + + void reset_moving_turtle( + const std::shared_ptr request, + std::shared_ptr response); +}; +} + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/rotate_turtle1.hpp b/software_training_assignment/include/rotate_turtle1.hpp new file mode 100644 index 0000000..cc15794 --- /dev/null +++ b/software_training_assignment/include/rotate_turtle1.hpp @@ -0,0 +1,22 @@ +#ifndef ROTATE_H +#define ROTATE_H + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include + +namespace composition +{ +class RotateTurtle : public rclcpp::Node +{ + public: + explicit RotateTurtle(const rclcpp::NodeOptions & options); + + private: + rclcpp::Publisher::SharedPtr publisher; + void callback(); +}; +} + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/spawn_turtle.hpp b/software_training_assignment/include/spawn_turtle.hpp new file mode 100644 index 0000000..047f1d3 --- /dev/null +++ b/software_training_assignment/include/spawn_turtle.hpp @@ -0,0 +1,30 @@ +#ifndef SPAWN_H +#define SPAWN_H + +#include +#include + +#include "turtlesim/srv/spawn.hpp" + +#include +#include +#include + +namespace composition +{ +class SpawnTurtle : public rclcpp::Node +{ + public: + SOFTWARE_TRAINING_PUBLIC + explicit SpawnTurtle(const rclcpp::NodeOptions &options); + + private: + rclcpp::Client::SharedPtr srv_client; + rclcpp::TimerBase::SharedPtr timer; + + SOFTWARE_TRAINING_LOCAL + void spawn_turtles(); +}; +} + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/stationary_position_publisher.hpp b/software_training_assignment/include/stationary_position_publisher.hpp new file mode 100644 index 0000000..812e536 --- /dev/null +++ b/software_training_assignment/include/stationary_position_publisher.hpp @@ -0,0 +1,29 @@ +#ifndef STAT_POS_H +#define STAT_POS_HS + +#include +#include + +#include +#include +#include +#include + +#include + +namespace composition +{ +class StationaryPosPublisher : public rclcpp::Node +{ + public: + StationaryPosPublisher(const rclcpp::NodeOptions &options); + + private: + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr pose_sub; + + void get_pose(const turtlesim::msg::Pose::SharedPtr currentPose); +}; +} + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/turtle_info.hpp b/software_training_assignment/include/turtle_info.hpp new file mode 100644 index 0000000..ba66ef6 --- /dev/null +++ b/software_training_assignment/include/turtle_info.hpp @@ -0,0 +1,13 @@ +#ifndef TURTLE_INFO_H +#define TURTLE_INFO_H + +#include +#include + + +std::map> turtles { + { "moving_turtle", { 10, 10} }, + { "stationary_turtle", { 5, 5 } } +}; + +#endif \ No newline at end of file diff --git a/software_training_assignment/include/visibility.h b/software_training_assignment/include/visibility.h new file mode 100644 index 0000000..939e175 --- /dev/null +++ b/software_training_assignment/include/visibility.h @@ -0,0 +1,51 @@ +#ifndef SOFTWARE_TRAINING__VISIBILITY_H_ +#define SOFTWARE_TRAINING__VISIBILITY_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_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_H_ \ No newline at end of file diff --git a/software_training_assignment/launch/__pycache__/launch.cpython-38.pyc b/software_training_assignment/launch/__pycache__/launch.cpython-38.pyc new file mode 100644 index 0000000..c48f831 Binary files /dev/null and b/software_training_assignment/launch/__pycache__/launch.cpython-38.pyc differ diff --git a/software_training_assignment/launch/__pycache__/training.launch.cpython-38.pyc b/software_training_assignment/launch/__pycache__/training.launch.cpython-38.pyc new file mode 100644 index 0000000..b0e2eb5 Binary files /dev/null and b/software_training_assignment/launch/__pycache__/training.launch.cpython-38.pyc differ diff --git a/software_training_assignment/launch/training.launch.py b/software_training_assignment/launch/training.launch.py new file mode 100644 index 0000000..7908eac --- /dev/null +++ b/software_training_assignment/launch/training.launch.py @@ -0,0 +1,21 @@ +import launch +from launch_ros.actions import Node + + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + return launch.LaunchDescription([ + Node( + package='turtlesim', + namespace='turtlesim2', + executable='turtlesim_node', + name='turtlesim' + ), + Node( + package='software_training_assignment', + namespace='composition', + executable='spawn', + name='spawn' + )]) \ No newline at end of file diff --git a/software_training_assignment/msg/Position.msg b/software_training_assignment/msg/Position.msg new file mode 100644 index 0000000..c685ffe --- /dev/null +++ b/software_training_assignment/msg/Position.msg @@ -0,0 +1,3 @@ +float64 x +float64 y +float64 distance \ No newline at end of file diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml new file mode 100644 index 0000000..f264743 --- /dev/null +++ b/software_training_assignment/package.xml @@ -0,0 +1,29 @@ + + + + software_training_assignment + 0.0.0 + TODO: Package description + meshva + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + turtlesim + std_msgs + + builtin_interfaces + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/software_training_assignment/src/clear_turtle.cpp b/software_training_assignment/src/clear_turtle.cpp new file mode 100644 index 0000000..2ad444d --- /dev/null +++ b/software_training_assignment/src/clear_turtle.cpp @@ -0,0 +1,42 @@ +#include "clear_turtle.hpp" +#include + +namespace composition +{ + +ClearTurtle::ClearTurtle(const rclcpp::NodeOptions & options) + : Node("clear_turtle_client") +{ + RCLCPP_INFO(this->get_logger(), "Here 1"); + + this->client = this->create_client("kill"); + + this-> timer = this->create_wall_timer(std::chrono::seconds(1), + std::bind(&ClearTurtle::kill_turtles, this)); +} + +void ClearTurtle::kill_turtles() +{ + RCLCPP_INFO(this->get_logger(), "Here 2"); + + std::map>::iterator it; + + for (it = turtles.begin(); it != turtles.end(); it++) + { + std::unique_ptr request = + std::make_unique(); + + request->name = it->first; + + auto end = [this] (rclcpp::Client::SharedFuture response) + -> void { + rclcpp::shutdown(); + }; + + this->client->async_send_request(std::move(request), end); + } +} + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::ClearTurtle) \ No newline at end of file diff --git a/software_training_assignment/src/move_turtle_server.cpp b/software_training_assignment/src/move_turtle_server.cpp new file mode 100644 index 0000000..2653231 --- /dev/null +++ b/software_training_assignment/src/move_turtle_server.cpp @@ -0,0 +1,100 @@ +#include "move_turtle_server.hpp" + +using namespace std::placeholders; +using namespace std::chrono; + +namespace composition +{ + +MoveTurtleServer::MoveTurtleServer(const rclcpp::NodeOptions & options) + : Node("move_turtle") +{ + RCLCPP_INFO(this->get_logger(), "MOVE"); + + this->publisher = this->create_publisher("moving_turtle/cmd_vel", 10); + + this->action_server = rclcpp_action::create_server( + this, + "move_to", + std::bind(&MoveTurtleServer::handle_goal, this, _1, _2), + std::bind(&MoveTurtleServer::handle_cancel, this, _1), + std::bind(&MoveTurtleServer::handle_accepted, this, _1) + ); + + pose_sub = this->create_subscription("moving_turtle/pose",0, + std::bind(&MoveTurtleServer::get_pose, this, _1)); +} + +rclcpp_action::GoalResponse MoveTurtleServer::handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) +{ + target = goal->position; + RCLCPP_INFO(this->get_logger(), "Received goal request with position %f, %f", target[0], target[1]); + (void) uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + + +rclcpp_action::CancelResponse MoveTurtleServer::handle_cancel( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void) goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MoveTurtleServer::handle_accepted(const std::shared_ptr goal_handle) +{ + std::thread{std::bind(&MoveTurtleServer::execute, this, _1), goal_handle}.detach(); +} + +void MoveTurtleServer::execute(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Rate loop_rate(100); + + int linear_velocity = 1; //change + RCLCPP_INFO(this->get_logger(), "Current position: %f, %f \n Target position: %f, %f", + curr_x, curr_y, target[0], target[1]); + + + auto msg = geometry_msgs::msg::Twist(); + msg.linear.x = (target[0] - curr_x) * linear_velocity; + msg.linear.y = (target[1] - curr_y) * linear_velocity; + publisher->publish(msg); + + auto feedback = std::make_shared(); + auto& distance = feedback->distance; + auto result = std::make_shared(); + + auto start = duration_cast(system_clock::now().time_since_epoch()).count(); + + while (rclcpp::ok() && std::abs(curr_x-target[0] > 0.05) && std::abs(curr_y-target[1]) > 0.05) + { + distance = std::sqrt(std::pow(curr_x-target[0], 2) + std::pow(curr_y-target[1], 2)); + + goal_handle->publish_feedback(feedback); + loop_rate.sleep(); + } + + if (rclcpp::ok()) + { + auto now = duration_cast(system_clock::now().time_since_epoch()).count(); + result->seconds = now - start; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } +} + +void MoveTurtleServer::get_pose(const turtlesim::msg::Pose::SharedPtr currentPose) +{ + this->curr_x = currentPose->x; + this->curr_y = currentPose->y; + + //RCLCPP_INFO(this->get_logger(), "Current position: %f, %f", curr_x, curr_y); +} + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::MoveTurtleServer) \ No newline at end of file diff --git a/software_training_assignment/src/reset_moving.cpp b/software_training_assignment/src/reset_moving.cpp new file mode 100644 index 0000000..d24b595 --- /dev/null +++ b/software_training_assignment/src/reset_moving.cpp @@ -0,0 +1,57 @@ +#include "reset_moving.hpp" + +using namespace std::placeholders; +using namespace std::chrono_literals; + +namespace composition +{ + +ResetMoving::ResetMoving(const rclcpp::NodeOptions & options) + : Node("reset_moving") +{ + this->teleport_cli = this->create_client("moving_turtle/teleport_absolute"); + + RCLCPP_INFO(this->get_logger(), "HERE RESET"); + + this->teleport_srv = this->create_service + ("moving_turtle_reset", + std::bind(&ResetMoving::reset_moving_turtle, this, _1, _2)); +} + + +void ResetMoving::reset_moving_turtle( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!teleport_cli->wait_for_service(2s)) + { + RCLCPP_INFO(this->get_logger(), "Try again"); + response->output = false; + return; + } + + std::shared_ptr teleport_request = + std::make_shared(); + + teleport_request->x = turtles.find("moving_turtle")->second[0]; + teleport_request->y = turtles.find("moving_turtle")->second[1]; + teleport_request->theta = 0; + + RCLCPP_INFO(this->get_logger(), "x: " + std::to_string(teleport_request->x) + + " y: " + std::to_string(teleport_request->y)); + + auto end = [this] (rclcpp::Client::SharedFuture future) + -> void { + RCLCPP_INFO(this->get_logger(), "done"); + rclcpp::shutdown(); + }; + + auto result = teleport_cli->async_send_request(teleport_request, end); + RCLCPP_INFO(this->get_logger(), "sent"); + + response->output = true; +} + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::ResetMoving) \ No newline at end of file diff --git a/software_training_assignment/src/rotate_turtle1.cpp b/software_training_assignment/src/rotate_turtle1.cpp new file mode 100644 index 0000000..c4a9188 --- /dev/null +++ b/software_training_assignment/src/rotate_turtle1.cpp @@ -0,0 +1,29 @@ +#include "rotate_turtle1.hpp" + +namespace composition +{ + +RotateTurtle::RotateTurtle(const rclcpp::NodeOptions & options) + : Node("rotate_turtle") +{ + this->publisher = this->create_publisher("turtle1/cmd_vel", 10); + callback(); +} + +void RotateTurtle::callback() +{ + RCLCPP_INFO(this->get_logger(), "Here"); + + auto msg = geometry_msgs::msg::Twist(); + + auto ang = geometry_msgs::msg::Vector3(); + ang.z = 5; + + msg.angular = ang; + + publisher->publish(msg); +} + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::RotateTurtle) \ No newline at end of file diff --git a/software_training_assignment/src/spawn_turtle.cpp b/software_training_assignment/src/spawn_turtle.cpp new file mode 100644 index 0000000..d0a8fab --- /dev/null +++ b/software_training_assignment/src/spawn_turtle.cpp @@ -0,0 +1,54 @@ +#include "spawn_turtle.hpp" + +using namespace std::chrono_literals; + +namespace composition +{ + +SpawnTurtle::SpawnTurtle(const rclcpp::NodeOptions &options) + : Node("spawn_turtle") +{ + std::this_thread::sleep_for(2s); + + RCLCPP_INFO(this->get_logger(), "HERE"); + + this->srv_client = this->create_client("/spawn"); + + this->timer = this->create_wall_timer(std::chrono::milliseconds(50), + std::bind(&SpawnTurtle::spawn_turtles, this)); + +} + +void SpawnTurtle::spawn_turtles() +{ + if (!srv_client->wait_for_service(2s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Service was interupted - EXIT"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Waiting for service - DNE - EXIT"); + } + + std::map>::iterator it; + + for (it = turtles.begin(); it != turtles.end(); it++) + { + std::unique_ptr request = + std::make_unique(); + + request->name = it->first; + request->x = it->second[0]; + request->y = it->second[1]; + + auto end = [this] (rclcpp::Client::SharedFuture response) + -> void { + rclcpp::shutdown(); + }; + + this->srv_client->async_send_request(std::move(request), end); + } +} + +} +RCLCPP_COMPONENTS_REGISTER_NODE(composition::SpawnTurtle) \ No newline at end of file diff --git a/software_training_assignment/src/stationary_position_publisher.cpp b/software_training_assignment/src/stationary_position_publisher.cpp new file mode 100644 index 0000000..a8de692 --- /dev/null +++ b/software_training_assignment/src/stationary_position_publisher.cpp @@ -0,0 +1,43 @@ +#include "stationary_position_publisher.hpp" + +using namespace std::placeholders; + +namespace composition +{ + +StationaryPosPublisher::StationaryPosPublisher(const rclcpp::NodeOptions &options) + : Node("stationary_position_publisher", options) +{ + RCLCPP_INFO(this->get_logger(), "HERE"); + publisher = this->create_publisher("pos_dis", 10); + + pose_sub = this->create_subscription("moving_turtle/pose",0, + std::bind(&StationaryPosPublisher::get_pose, this, _1)); + +} + +void StationaryPosPublisher::get_pose(const turtlesim::msg::Pose::SharedPtr currentPose) { + + float mov_x = currentPose->x; + float mov_y = currentPose->y; + + float stat_x = turtles.find("stationary_turtle")->second[0]; + float stat_y = turtles.find("stationary_turtle")->second[1]; + + float distance = pow(pow(mov_x - stat_x, 2) + pow(mov_y - stat_y, 2), 0.5); + + auto message = software_training_assignment::msg::Position(); + message.x = stat_x; + message.y = stat_y; + message.distance = distance; + + publisher->publish(message); + + RCLCPP_INFO(this->get_logger(), std::to_string(distance)); + + rclcpp::shutdown(); +} + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::StationaryPosPublisher) \ No newline at end of file diff --git a/software_training_assignment/srv/ExampleService.srv b/software_training_assignment/srv/ExampleService.srv new file mode 100644 index 0000000..e99f06d --- /dev/null +++ b/software_training_assignment/srv/ExampleService.srv @@ -0,0 +1,2 @@ +--- +bool output \ No newline at end of file