diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..1a28943 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,162 @@ +cmake_minimum_required(VERSION 3.5) +project(software_training_assignment) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(turtlesim REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(rcutils REQUIRED) +find_package(rcl REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# setup interfaces +# set(srv_files +# "" +# ) + +set (msg_files + "msg/Distances.msg" +) + +set(action_files + "action/Move.action" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${action_files} + DEPENDENCIES std_msgs geometry_msgs builtin_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) + +#include the 'include' directory +include_directories(include) + +#create resource which references the libraries in the binary bin +set(node_plugins "") + +#add plugins as SHARED library + +#add clear_turtle_node as a plugin +add_library(clear_request SHARED + src/clear_turtles.cpp) +target_compile_definitions(clear_request + PRIVATE "SOFTWARE_TRAINING_DLL") +ament_target_dependencies(clear_request + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_srvs") +# another way of registering and setting the node as a component below +# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear) +rclcpp_components_register_nodes(clear_request "composition::clear_turtles") +# this way we can execute the component with - ros2 component standalone software_training_assignment composition::clear_turtles +set(node_plugins "${node_plugins}composition::clear_turtles;$\n") + +add_library(spawn_turtles SHARED + src/spawn_turtles.cpp) +target_compile_definitions(spawn_turtles + PRIVATE "SOFTWARE_TRAINING_DLL") +ament_target_dependencies(spawn_turtles + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_srvs") +# another way of registering and setting the node as a component below +# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear) +rclcpp_components_register_nodes(spawn_turtles "composition::SpawnTwoTurtles") +# this way we can execute the component with - ros2 component standalone software_training_assignment composition::SpawnTwoTurtles +set(node_plugins "${node_plugins}composition::SpawnTwoTurtles;$\n") + + +add_library(move_circular SHARED + src/move_circular.cpp) +target_compile_definitions(move_circular + PRIVATE "SOFTWARE_TRAINING_DLL") +ament_target_dependencies(move_circular + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_srvs" + "geometry_msgs" + "std_msgs") +# another way of registering and setting the node as a component below +# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear) +rclcpp_components_register_nodes(move_circular "composition::CircularMotion") +# this way we can execute the component with - ros2 component standalone software_training_assignment composition::SpawnTwoTurtles +set(node_plugins "${node_plugins}composition::CircularMotion;$\n") + +add_library(reset_moving SHARED + src/reset_moving_turtle.cpp) +target_compile_definitions(reset_moving + PRIVATE "SOFTWARE_TRAINING_DLL") +ament_target_dependencies(reset_moving + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_srvs") +# another way of registering and setting the node as a component below +# rclcpp_components_register_nodes(clear_request PLUGIN "composition::clear_turtles" EXECUTABLE request_clear) +rclcpp_components_register_nodes(reset_moving "composition::MotionReset") +# this way we can execute the component with - ros2 component standalone software_training_assignment composition::SpawnTwoTurtles +set(node_plugins "${node_plugins}composition::MotionReset;$\n") + + +add_library(distance_tracker SHARED + src/distance.cpp) +target_compile_definitions(distance_tracker + PRIVATE "SOFTWARE_TRAINING_DLL") +ament_target_dependencies(distance_tracker + "rclcpp" + "rclcpp_components" + "turtlesim") +# build ROS custom messages +rosidl_target_interfaces(distance_tracker ${PROJECT_NAME} "rosidl_typesupport_cpp") +rclcpp_components_register_nodes(distance_tracker "composition::DistanceInfo") +set(node_plugins "${node_plugins}composition::DistanceInfo;$\n") + +add_library(move_action_server SHARED + src/move_action.cpp) +target_compile_definitions(move_action_server + PRIVATE "SOFTWARE_TRAINING_DLL") +ament_target_dependencies(move_action_server + "rclcpp" + "rclcpp_components" + "turtlesim" + "rclcpp_action" + "geometry_msgs") +# build ROS custom messages +rosidl_target_interfaces(move_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp") +rclcpp_components_register_nodes(move_action_server "composition::MoveAction") +set(node_plugins "${node_plugins}composition::MoveAction;$\n") + + +#tell where to put binaries +install(TARGETS + clear_request + spawn_turtles + move_circular + reset_moving + distance_tracker + move_action_server + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_package() diff --git a/action/Move.action b/action/Move.action new file mode 100644 index 0000000..ee25894 --- /dev/null +++ b/action/Move.action @@ -0,0 +1,9 @@ +# req +float32 x_pos +float32 y_pos +--- +# res +uint64 time_taken +--- +# feedback +float32 dist_to_goal \ No newline at end of file diff --git a/include/software_training_assignment/clear_turtles.hpp b/include/software_training_assignment/clear_turtles.hpp new file mode 100644 index 0000000..16ebb27 --- /dev/null +++ b/include/software_training_assignment/clear_turtles.hpp @@ -0,0 +1,34 @@ +// #ifndef CLEAR_TURTLES_HPP_ +// #define CLEAR_TURTLES_HPP_ + +// #include +// #include +// #include +// #include + +// #include + +// #include +// #include + +// namespace composition { + +// class turtle_service_request_node : public rclcpp::Node { +// public: +// SOFTWARE_TRAINING_PUBLIC +// explicit turtle_service_request_node(const rclcpp::NodeOptions &options); + +// private: +// rclcpp::Client::SharedPtr client; +// rclcpp::TimerBase::SharedPtr timer; + +// // all the turtles +// std::vector turtle_names = {"turtle1", "moving_turtle", +// "stationary_turtle"}; + +// SOFTWARE_TRAINING_LOCAL +// void kill(); +// }; + +// } // namespace composition +// #endif // TURTLE_SERVICE_REQUEST_NODE_HPP_ \ No newline at end of file diff --git a/include/software_training_assignment/common.hpp b/include/software_training_assignment/common.hpp new file mode 100644 index 0000000..b2f3d36 --- /dev/null +++ b/include/software_training_assignment/common.hpp @@ -0,0 +1,11 @@ +#ifndef COMMON_H +#define COMMON_H + +// define common structs here +struct Position { + float x; + float y; + float theta; +}; + +#endif \ No newline at end of file diff --git a/include/software_training_assignment/distance.hpp b/include/software_training_assignment/distance.hpp new file mode 100644 index 0000000..ee09ef3 --- /dev/null +++ b/include/software_training_assignment/distance.hpp @@ -0,0 +1,59 @@ +#ifndef DISTANCE_H +#define DISTANCE_H + +#include +#include + +#include + +#include +#include +#include +#include + +// using namespace std::chrono_literals; +// using namespace std::placeholders; + + +namespace composition { + class DistanceInfo: public rclcpp::Node { + public: + using pose_msg = turtlesim::msg::Pose; + using distance_msg = software_training_assignment::msg::Distances; + + SOFTWARE_TRAINING_PUBLIC + explicit DistanceInfo(const rclcpp::NodeOptions &options); + + private: + rclcpp::Publisher::SharedPtr distance_pub; + rclcpp::TimerBase::SharedPtr pub_timer; + rclcpp::Subscription::SharedPtr moving_pos_sub; + rclcpp::Subscription::SharedPtr static_pos_sub; + + // create threads to run callbacks + rclcpp::CallbackGroup::SharedPtr callbacks; + + // struct Position { + // float x; + // float y; + // float theta; + // }; + + Position moving_turtle_pos; + Position static_turtle_pos; + bool read_moving, read_static; + + SOFTWARE_TRAINING_LOCAL + // timer call back to get current positions and compute distances + void publish_distances(); + // subscriber functions to get data from topic + void get_moving_position(const pose_msg::SharedPtr msg); + void get_static_position(const pose_msg::SharedPtr msg); + + // helper function for computing euclidean distance + double compute_euc_dist(); + + }; +} + +#endif \ No newline at end of file diff --git a/include/software_training_assignment/move_action.hpp b/include/software_training_assignment/move_action.hpp new file mode 100644 index 0000000..9d8b9d6 --- /dev/null +++ b/include/software_training_assignment/move_action.hpp @@ -0,0 +1,56 @@ +#ifndef DISTANCE_H +#define DISTANCE_H + +#include +#include + +#include +#include "rclcpp_action/rclcpp_action.hpp" + +#include +#include + +#include +#include +#include "software_training_assignment/action/move.hpp" + + +namespace composition { +class MoveAction: public rclcpp::Node { +public: + using motion_action = software_training_assignment::action::Move; + using motion_server = rclcpp_action::Server; + using goal_handle_ns = rclcpp_action::ServerGoalHandle; + using pose_msg = turtlesim::msg::Pose; + using twist_msg = geometry_msgs::msg::Twist; + SOFTWARE_TRAINING_PUBLIC + explicit MoveAction(const rclcpp::NodeOptions &options); + +private: + motion_server::SharedPtr move_server; + rclcpp::Publisher::SharedPtr do_move_pub; + rclcpp::Subscription::SharedPtr pos_sub; + + Position curr_pos; + Position goal_pos; + // constant values to play around with + const float SMALL_DIST = 0.1; + const float MOVE_SCALE = 0.5; + + void get_pos(const pose_msg::SharedPtr msg); + float euc_dist(); + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &uuid, + std::shared_ptr 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); + +}; +} + +#endif \ No newline at end of file diff --git a/include/software_training_assignment/visibility.h b/include/software_training_assignment/visibility.h new file mode 100644 index 0000000..907a909 --- /dev/null +++ b/include/software_training_assignment/visibility.h @@ -0,0 +1,65 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/launch/launch.py b/launch/launch.py new file mode 100644 index 0000000..fc03f25 --- /dev/null +++ b/launch/launch.py @@ -0,0 +1,41 @@ +from http.server import executable +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node + +def generate_launch_description(): + node = Node( + package='turtlesim', + namespace="", + executable="turtlesim_node" + ) + + container = ComposableNodeContainer( + name='container', + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="software_training_assignment", + plugin="composition::SpawnTwoTurtles"), + ComposableNode( + package="software_training_assignment", + plugin="composition::clear_turtles"), + ComposableNode( + package="software_training_assignment", + plugin="composition::MoveAction"), + ComposableNode( + package="software_training_assignment", + plugin="composition::MotionReset"), + ComposableNode( + package="software_training_assignment", + plugin="composition::DistanceInfo"), + ComposableNode( + package="software_training_assignment", + plugin="composition::CircularMotion") + ] + ) + + return launch.LaunchDescription([node, container]) \ No newline at end of file diff --git a/msg/Distances.msg b/msg/Distances.msg new file mode 100644 index 0000000..56e9044 --- /dev/null +++ b/msg/Distances.msg @@ -0,0 +1,3 @@ +float32 x_dist +float32 y_dist +float32 euc_dist \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..57695f1 --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + + software_training_assignment + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + rclcpp + rclcpp_action + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/clear_turtles.cpp b/src/clear_turtles.cpp new file mode 100644 index 0000000..f171114 --- /dev/null +++ b/src/clear_turtles.cpp @@ -0,0 +1,88 @@ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std::chrono_literals; + +namespace composition { + +using namespace std::placeholders; +class clear_turtles : public rclcpp::Node { +public: + SOFTWARE_TRAINING_PUBLIC + explicit clear_turtles(const rclcpp::NodeOptions &options): Node("clear_turtle_node", options) { + // create the client to call the kill service + client = create_client("/kill"); + // create a service to call the kill client + clear_server = create_service("remove_all_turtles", std::bind(&clear_turtles::clear, this, _1, _2)); + // create a service to add new turtles when spawning (to know which turtles to kill when clearing) + add_name_server = create_service("add_turtle_name", std::bind(&clear_turtles::add_name, this, _1, _2)); + } + +private: + rclcpp::Service::SharedPtr clear_server; + rclcpp::Client::SharedPtr client; + // create a service to add turtles to the list of turtles + rclcpp::Service::SharedPtr add_name_server; + + // clear the initial turtle which is launched with the turtle node + std::vector turtle_names = {"turtle1", "moving_turtle", "stationary_turtle"}; + // , "moving_turtle", "stationary_turtle"}; + + SOFTWARE_TRAINING_LOCAL + + void add_name(const std::shared_ptr req, + const std::shared_ptr /*resp */) { + turtle_names.push_back(req->name); + } + + void kill_callback(rclcpp::Client::SharedFuture resp) { + (void)resp; + RCLCPP_INFO(this->get_logger(), "Turtle killed!"); + // stop executing callback + rclcpp::shutdown(); + } + + void clear(const std::shared_ptr req, + std::shared_ptr resp) { + + (void)req; + if (!client->wait_for_service(2s)) { + if(!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for kill service, exiting"); + resp->success = false; + return; + } + RCLCPP_INFO(this->get_logger(), "Service not avaliable"); + resp->success = false; + return; + } + + for (auto name: turtle_names) { + auto kill_req = std::make_shared(); + kill_req->name = name; + // send request to kill turtle + // auto res = client->async_send_request(kill_req, std::bind(&clear_turtles::kill_callback, this, _1)); + auto res = client->async_send_request(kill_req); + } + // too much work, just leave it - remove all turtles after clearing them to update the known turtles + // turtle_names.clear(); + resp->success = true; + + } +}; + +} // namespace composition + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::clear_turtles) diff --git a/src/distance.cpp b/src/distance.cpp new file mode 100644 index 0000000..8ec735c --- /dev/null +++ b/src/distance.cpp @@ -0,0 +1,76 @@ + +#include +#include + +// using pose_msg = turtlesim::msg::Pose; +// using distance_msg = software_training_assignment::msg::Distances; +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + +DistanceInfo::DistanceInfo(const rclcpp::NodeOptions &options): Node("distances_node", options) { + // make mutex for subscribers and timer (since callback functions should hold lock) + callbacks = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // callback thread + auto callback_options = rclcpp::SubscriptionOptions(); + callback_options.callback_group = callbacks; + distance_pub = create_publisher("turtle_distances", 10); + moving_pos_sub = create_subscription("moving_turtle/pose", 10, std::bind(&DistanceInfo::get_moving_position, this, _1), callback_options); + static_pos_sub = create_subscription("stationary_turtle/pose", 10, std::bind(&DistanceInfo::get_static_position, this, _1), callback_options); + pub_timer = create_wall_timer(500ms, std::bind(&DistanceInfo::publish_distances, this), callbacks); + read_moving = false; + read_static = false; +} + +void DistanceInfo::publish_distances() { + if (read_moving && read_static) { + // compute the distances (stationary to moving) + float x_dist = moving_turtle_pos.x - static_turtle_pos.x; + float y_dist = moving_turtle_pos.y - static_turtle_pos.y; + double euc_dist = compute_euc_dist(); + + // publish distance message + auto dist_msg = distance_msg(); + dist_msg.x_dist = x_dist; + dist_msg.y_dist = y_dist; + dist_msg.euc_dist = euc_dist; + RCLCPP_INFO(this->get_logger(), "Publishing distances: x %f | y %f | euclidean %f", dist_msg.x_dist, dist_msg.y_dist, dist_msg.euc_dist); + distance_pub->publish(dist_msg); + + } else { + RCLCPP_WARN(this->get_logger(), "No position for either moving or static turtle, distances not computed"); + } +} + +void DistanceInfo::get_moving_position(const pose_msg::SharedPtr msg) { + // store the moving turtle position + moving_turtle_pos.x = msg->x; + moving_turtle_pos.y = msg->y; + moving_turtle_pos.theta = msg->theta; + // RCLCPP_INFO(this->get_logger(), "Read moving pos: x %f | y %f | theta %f", moving_turtle_pos.x, moving_turtle_pos.y, moving_turtle_pos.theta); + read_moving = true; +} + +void DistanceInfo::get_static_position(const pose_msg::SharedPtr msg) { + // store the stationary turtle position + static_turtle_pos.x = msg->x; + static_turtle_pos.y = msg->y; + static_turtle_pos.theta = msg->theta; + // RCLCPP_INFO(this->get_logger(), "Read static pos: x %f | y %f | theta %f", static_turtle_pos.x, static_turtle_pos.y, static_turtle_pos.theta); + read_static = true; +} + +double DistanceInfo::compute_euc_dist() { + double x_euc = std::pow((moving_turtle_pos.x - static_turtle_pos.x), 2); + double y_euc = std::pow((moving_turtle_pos.y - static_turtle_pos.y), 2); + double res = std::sqrt(x_euc + y_euc); + return res; +} + +} + + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::DistanceInfo) \ No newline at end of file diff --git a/src/move_action.cpp b/src/move_action.cpp new file mode 100644 index 0000000..e8dd7bd --- /dev/null +++ b/src/move_action.cpp @@ -0,0 +1,130 @@ +#include + +#include +#include + +namespace composition { + +MoveAction::MoveAction(const rclcpp::NodeOptions &options): Node("action_server_node", options) +{ + using namespace std::placeholders; + move_server = rclcpp_action::create_server( + this, + "waypoint_move", + std::bind(&MoveAction::handle_goal, this, _1, _2), + std::bind(&MoveAction::handle_cancel, this, _1), + std::bind(&MoveAction::handle_accepted, this, _1)); + + do_move_pub = create_publisher("/moving_turtle/cmd_vel", 10); + pos_sub = create_subscription("/moving_turtle/pose", 10, std::bind(&MoveAction::get_pos, this, _1)); +} + +void MoveAction::get_pos(const pose_msg::SharedPtr msg) { + // receive the messages being published by the /pose topic + curr_pos.x = msg->x; + curr_pos.y = msg->y; +} + +float MoveAction::euc_dist() { + float x_dist = goal_pos.x - curr_pos.x; + float y_dist = goal_pos.y - curr_pos.y; + return std::sqrt(std::pow(x_dist, 2) + std::pow(y_dist, 2)); +} + + +rclcpp_action::GoalResponse MoveAction::handle_goal( + const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) +{ + // accept all goals - could also do error checking + RCLCPP_INFO(this->get_logger(), "Recevied goal request with waypoint x: %f and y: %f", goal->x_pos, goal->y_pos); + goal_pos.x = goal->x_pos; + goal_pos.y = goal->y_pos; + (void)uuid; + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + +} + +rclcpp_action::CancelResponse MoveAction::handle_cancel(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MoveAction::handle_accepted(const std::shared_ptr goal_handle) +{ + // create a thread to return quickly and not block + std::thread{std::bind(&MoveAction::execute, this, std::placeholders::_1), goal_handle}.detach(); +} + +void MoveAction::execute(const std::shared_ptr goal_handle) +{ + // get node current time + rclcpp::Time start_time = this->now(); + RCLCPP_INFO(this->get_logger(), "Executing goal to x: %f and y: %f", goal_pos.x, goal_pos.y); + // set frequency for goal execution (turtle to move) + rclcpp::Rate action_rate(1); + auto feedback_ptr = std::make_shared(); + // make a reference to the distance to easily change the distance for the feedback ptr + auto & feedback_dist = feedback_ptr->dist_to_goal; + // setup the result (time taken) + auto res_ptr = std::make_shared(); + auto &res_time = res_ptr->time_taken; + + // loop here + while(rclcpp::ok() && euc_dist() > SMALL_DIST) { + // handle cancel action + if (goal_handle->is_canceling()) { + rclcpp::Time curr_time = this->now(); + rclcpp::Duration partial_time_taken = curr_time - start_time; + // convert time to uint64 + long int partial_duration_ns = partial_time_taken.nanoseconds(); + res_time = partial_duration_ns * std::pow(10, -9); + // set final result + goal_handle->canceled(res_ptr); + RCLCPP_INFO(this->get_logger(), "Action cancelled"); + return; + } + // move the turtle in the correct direction (calculate the movement 'slope') + // note: this works assuming theta=0 so we are in normal XY reference frame + float x_scaled = (goal_pos.x - curr_pos.x)*MOVE_SCALE; + float y_scaled = (goal_pos.y - curr_pos.y)*MOVE_SCALE; + auto move_msg = twist_msg(); + move_msg.linear.x = x_scaled; + move_msg.linear.y = y_scaled; + move_msg.linear.z = 0; + move_msg.angular.x = 0; + move_msg.angular.y = 0; + move_msg.angular.z = 0; + do_move_pub->publish(move_msg); + + // update the distance feedback + feedback_dist = euc_dist(); + goal_handle->publish_feedback(feedback_ptr); + // stall the main action loop before continuing + action_rate.sleep(); + } + + // handle action completion + if (rclcpp::ok()) { + rclcpp::Time end = this->now(); + rclcpp::Duration time_taken = end - start_time; + // convert time to uint64 + long int duration_ns = time_taken.nanoseconds(); + res_time = duration_ns * std::pow(10, -9); + + // set final result + goal_handle->succeed(res_ptr); + RCLCPP_INFO(this->get_logger(), "Action completed"); + } + +} + +} + + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::MoveAction) \ No newline at end of file diff --git a/src/move_circular.cpp b/src/move_circular.cpp new file mode 100644 index 0000000..ddb7c3a --- /dev/null +++ b/src/move_circular.cpp @@ -0,0 +1,60 @@ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + class CircularMotion: public rclcpp::Node { + using trigger_srv = std_srvs::srv::Trigger; + using twist_msg = geometry_msgs::msg::Twist; + + public: + SOFTWARE_TRAINING_PUBLIC + explicit CircularMotion(const rclcpp::NodeOptions &options): Node("circular_motion_node", options) { + move_server = create_service("move_turtle1_circular", std::bind(&CircularMotion::move_circular, this, _1, _2)); + move_publisher = create_publisher("turtle1/cmd_vel", 10); + + } + private: + rclcpp::Service::SharedPtr move_server; // use the service to start the timer + rclcpp::Publisher::SharedPtr move_publisher; + rclcpp::TimerBase::SharedPtr move_timer; + + SOFTWARE_TRAINING_LOCAL + void move_circular(const std::shared_ptr /* req */, std::shared_ptr resp) { + // create the timer when the service is called + RCLCPP_INFO(this->get_logger(), "Creating turtle1 publishing timer"); + move_timer = create_wall_timer(1000ms, std::bind(&CircularMotion::timer_callback, this)); + resp->success = true; + } + + void timer_callback() { + // create the message + auto circle_msg = twist_msg(); + circle_msg.linear.x = 1; + circle_msg.linear.y = 0; + circle_msg.linear.z = 0; + circle_msg.angular.x = 0; + circle_msg.angular.y = 0; + circle_msg.angular.z = 1; + // consistently publish data to move in a circle (every second) + move_publisher->publish(circle_msg); + } + }; +} + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::CircularMotion) + diff --git a/src/reset_moving_turtle.cpp b/src/reset_moving_turtle.cpp new file mode 100644 index 0000000..002c043 --- /dev/null +++ b/src/reset_moving_turtle.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +// TODO: test this to make sure it works +namespace composition { + class MotionReset: public rclcpp::Node { + + public: + using teleport_srv = turtlesim::srv::TeleportAbsolute; + using trigger_srv = std_srvs::srv::Trigger; + + SOFTWARE_TRAINING_PUBLIC + explicit MotionReset(const rclcpp::NodeOptions &options): rclcpp::Node("reset_moving_turle_node", options) { + reset_moving_server = create_service("moving_turtle_reset", std::bind(&MotionReset::pos_reset, this, _1, _2)); + teleport_abs_client = create_client("/moving_turtle/teleport_absolute"); + } + private: + rclcpp::Service::SharedPtr reset_moving_server; + rclcpp::Client::SharedPtr teleport_abs_client; + Position init_moving_turtle_pos = { 9, 8, 0 }; // starting position for the moving turtle + + + SOFTWARE_TRAINING_LOCAL + void pos_reset(const std::shared_ptr /* req */, std::shared_ptr resp) { + if (!teleport_abs_client->wait_for_service(2s)) { + if(!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for teleport_absolute service, exiting"); + resp->success = false; + return; + } + RCLCPP_INFO(this->get_logger(), "Service teleport_absolute not avaliable"); + resp->success = false; + return; + } + auto teleport_req = std::make_shared(); + teleport_req->x = init_moving_turtle_pos.x; + teleport_req->y = init_moving_turtle_pos.y; + teleport_req->theta = init_moving_turtle_pos.theta; + auto res = teleport_abs_client->async_send_request(teleport_req); + resp->success = true; + resp->message = "Moving turtle position reset"; + return; + } + + }; +} + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::MotionReset) \ No newline at end of file diff --git a/src/spawn_turtles.cpp b/src/spawn_turtles.cpp new file mode 100644 index 0000000..8e9c2b8 --- /dev/null +++ b/src/spawn_turtles.cpp @@ -0,0 +1,75 @@ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +// #include + + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + +class SpawnTwoTurtles: public rclcpp::Node { +public: + SOFTWARE_TRAINING_PUBLIC + explicit SpawnTwoTurtles(const rclcpp::NodeOptions &options): Node("spawn_turtle_node", options) { + spawn_client = create_client("/spawn"); + // add_turtle_client = create_client("/add_turtle_name"); + // define the starting positions order { x, y, theta } + two_turtle_server = create_service("spawn_two_turtles", std::bind(&SpawnTwoTurtles::two_turtle_spawn, this, _1, _2)); + } + +private: + rclcpp::Client::SharedPtr spawn_client; + rclcpp::Service::SharedPtr two_turtle_server; + // rclcpp::Client::SharedPtr add_turtle_client; + bool did_spawn = false; + + // define positions here + Position init_moving_turtle_pos = { 9, 8, 0 }; + Position stationary_turtle_pos = { 5, 5, 0 }; + + SOFTWARE_TRAINING_LOCAL + + void two_turtle_spawn(const std::shared_ptr /*request*/, std::shared_ptr response) { + // if (!did_spawn) { + auto stationary_spawn_req = std::make_shared(); + stationary_spawn_req->name = "stationary_turtle"; + + stationary_spawn_req->x = stationary_turtle_pos.x; + stationary_spawn_req->y = stationary_turtle_pos.y; + stationary_spawn_req->theta = stationary_turtle_pos.theta; + auto stationary_spawn_res = spawn_client->async_send_request(stationary_spawn_req); + + auto moving_spawn_req = std::make_shared(); + moving_spawn_req->name = "moving_turtle"; + + moving_spawn_req->x = init_moving_turtle_pos.x; + moving_spawn_req->y = init_moving_turtle_pos.y; + moving_spawn_req->theta = init_moving_turtle_pos.theta; + auto moving_spawn_res = spawn_client->async_send_request(moving_spawn_req); + + did_spawn = true; + response->success = true; + // } else { + // RCLCPP_WARN(this->get_logger(), "Turtle spawn service already called"); + // } + + } + +}; + +} + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::SpawnTwoTurtles) \ No newline at end of file