diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..588b717 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +/build +/install +/log +/.vscode \ No newline at end of file diff --git a/README.md b/README.md index 77bcc77..eb1d401 100644 --- a/README.md +++ b/README.md @@ -1 +1,28 @@ -# software_challenge \ No newline at end of file +<<<<<<< HEAD +The training challenge consists of the following: + +That training relies heavily on components - as this is the newer and better way to go about ROS2 design + +Write 6 components that do the following: (Note: Some of the components do not need to be made in the following order) + + Clears any existing turtles + + Create a component that moves 'turtle1' in a circular motion + + Spawns a turtle named "stationary_turtle" at x = 5, y = 5 Spawns a second turtle named "moving_turtle" at x = 25, y = 10 + + Create a service that resets the "moving_turtle" to its starting position. The service response should be whether or not it was successful. + + Create a publisher that publishes a custom msg. This custom msgs should have 3 float fields that correspond with the x and y distances of "stationary_turtle" to "moving turtle", as well as the distance between the two turtles. + + Create an action that moves "moving_turtle" to a waypoint in a straight line by publishing geometry_msgs/Twist msgs to turtleX/cmd_vel.The action's goal command is an absolute position of the waypoint, feedback is distance to the goal, and result is the time it took to reach the goal. You should define a custom action file. + +Lastly, create a launch file that will start up all the components and the turtlesim node (configure the parameters of the turtlesim node to however you see fit). Ensure that the turtlesim node is launched first as the other components are dependent upon it. + + +After each package, build with `colcon build`, then run the particular service: +e.g `ros2 run challenge_package `, then call the service with `ros2 service call / std_srvs/srv/Trigger` (only if its a service) + +======= +# software_challenge +>>>>>>> upstream/master diff --git a/src/challenge_package/CMakeLists.txt b/src/challenge_package/CMakeLists.txt new file mode 100644 index 0000000..3917408 --- /dev/null +++ b/src/challenge_package/CMakeLists.txt @@ -0,0 +1,91 @@ +# compiles C++ packages +cmake_minimum_required(VERSION 3.8) +project(challenge_package) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +# need to import msg files like this (from tutorial of https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Single-Package-Define-And-Use-Interface.html) +set(msg_files "msg/Names.msg") +set(act_files "action/Move.action") + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${act_files} +) + +include_directories(include) +ament_export_dependencies(rosidl_default_runtime) + + +add_executable(clear_turtle_listener src/clear_turtles.cpp) +add_executable(circle_turtle_service src/move_circle.cpp) +add_executable(spawn_turtle_service src/spawn_turtles.cpp) +add_executable(reset_turtle_service src/reset_turtle.cpp) +add_executable(dist_publisher src/publisher_dist.cpp) +add_executable(move_action_server src/move_action_server.cpp) +add_executable(move_action_client src/move_action_client.cpp) + +# bc msg folder in the same package, so you need to link them with this +rosidl_target_interfaces(clear_turtle_listener ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(move_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_target_interfaces(move_action_client ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# check how msg files are imported here and do the same for action files + + + +# link the dependencies of clear_turtle_listener with relevant libraries that are mentioned in the file itself +ament_target_dependencies(clear_turtle_listener rclcpp std_msgs turtlesim) +ament_target_dependencies(circle_turtle_service rclcpp std_srvs geometry_msgs) +ament_target_dependencies(spawn_turtle_service rclcpp std_srvs turtlesim) +ament_target_dependencies(reset_turtle_service rclcpp std_srvs turtlesim) +ament_target_dependencies(dist_publisher rclcpp geometry_msgs turtlesim) +ament_target_dependencies(move_action_server rclcpp rclcpp_action geometry_msgs turtlesim) +ament_target_dependencies(move_action_client rclcpp rclcpp_action geometry_msgs turtlesim) + + + +install(TARGETS + clear_turtle_listener + circle_turtle_service + spawn_turtle_service + reset_turtle_service + dist_publisher + move_action_server + move_action_client + DESTINATION lib/${PROJECT_NAME}) + + + + +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/src/challenge_package/action/Move.action b/src/challenge_package/action/Move.action new file mode 100644 index 0000000..359d4bf --- /dev/null +++ b/src/challenge_package/action/Move.action @@ -0,0 +1,9 @@ +# Define the action goal +float32 x_pos +float32 y_pos +--- +# Define the action result +float32 time_taken +--- +# Define the action feedback +float32 distance_to_goal \ No newline at end of file diff --git a/src/challenge_package/include/challenge_package/common.hpp b/src/challenge_package/include/challenge_package/common.hpp new file mode 100644 index 0000000..0856a67 --- /dev/null +++ b/src/challenge_package/include/challenge_package/common.hpp @@ -0,0 +1,11 @@ +#ifndef COMMON_H +#define COMMON_H + +// move these to a central file and import +struct Position { + float x; + float y; + float theta; +}; + +#endif \ No newline at end of file diff --git a/src/challenge_package/launch/__pycache__/launch.cpython-38.pyc b/src/challenge_package/launch/__pycache__/launch.cpython-38.pyc new file mode 100644 index 0000000..1f46e16 Binary files /dev/null and b/src/challenge_package/launch/__pycache__/launch.cpython-38.pyc differ diff --git a/src/challenge_package/launch/launch.py b/src/challenge_package/launch/launch.py new file mode 100644 index 0000000..3e84284 --- /dev/null +++ b/src/challenge_package/launch/launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='turtlesim', + namespace='', + executable='turtlesim_node', + ), + Node( + package='challenge_package', + namespace='', + executable='clear_turtle_listener', + ), + Node( + package='challenge_package', + namespace='', + executable='circle_turtle_service', + ), + Node( + package='challenge_package', + namespace='', + executable='spawn_turtle_service', + ), + Node( + package='challenge_package', + namespace='', + executable='reset_turtle_service', + ), + Node( + package='challenge_package', + namespace='', + executable='dist_publisher', + ), + Node( + package='challenge_package', + namespace='', + executable='move_action_server', + ), + ]) \ No newline at end of file diff --git a/src/challenge_package/msg/Names.msg b/src/challenge_package/msg/Names.msg new file mode 100644 index 0000000..d370e0d --- /dev/null +++ b/src/challenge_package/msg/Names.msg @@ -0,0 +1 @@ +string[] names \ No newline at end of file diff --git a/src/challenge_package/package.xml b/src/challenge_package/package.xml new file mode 100644 index 0000000..23a5b93 --- /dev/null +++ b/src/challenge_package/package.xml @@ -0,0 +1,33 @@ + + + + challenge_package + 0.0.0 + TODO: Package description + yaybeno + TODO: License declaration + + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + rclcpp + rclcpp_action + rclcpp_components + turtlesim + std_msgs + std_srvs + rcutils + rcl + geometry_msgs + builtin_interfaces + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/challenge_package/src/clear_turtles.cpp b/src/challenge_package/src/clear_turtles.cpp new file mode 100644 index 0000000..e5b73a7 --- /dev/null +++ b/src/challenge_package/src/clear_turtles.cpp @@ -0,0 +1,46 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "turtlesim/srv/kill.hpp" +#include "challenge_package/msg/names.hpp" + +using std::placeholders::_1; + +class KillTurtlesSubscriber : public rclcpp::Node +{ + public: + KillTurtlesSubscriber() + : Node("kill_turtle_subscriber") + { + client = create_client("/kill"); + subscription_ = this->create_subscription( + "kill_turtles", 10, std::bind(&KillTurtlesSubscriber::topic_callback, this, _1)); + } + + private: + rclcpp::Client::SharedPtr client; + void topic_callback(const challenge_package::msg::Names& msg) const + { + for (unsigned int i = 0; i < sizeof(msg); i++) { + const std::string name = msg.names[i]; + + // auto is for typing anything with a complex type + auto kill_req = std::make_shared(); + kill_req->name = name; + auto res = client->async_send_request(kill_req); + + RCLCPP_INFO(this->get_logger(), "Killed Turtle: '%s'", name.c_str()); + } + } + rclcpp::Subscription::SharedPtr subscription_; // sharedptr will share stuff by memory, but no big advantage (allegedly) + +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/challenge_package/src/move_action_client.cpp b/src/challenge_package/src/move_action_client.cpp new file mode 100644 index 0000000..9ffcf9f --- /dev/null +++ b/src/challenge_package/src/move_action_client.cpp @@ -0,0 +1,98 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "challenge_package/action/move.hpp" + +using move_action = challenge_package::action::Move; +using goal_handle_move = rclcpp_action::ClientGoalHandle; + +class MoveActionClient : public rclcpp::Node +{ +public: + MoveActionClient() + : Node("move_action_client") + { + this->client_ptr = rclcpp_action::create_client( + this, + "move_action" + ); + + // Wait for action server to be available + while (!client_ptr->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_INFO(this->get_logger(), "Action server not available. Waiting..."); + } + + this->send_goal(); + } + +private: + rclcpp_action::Client::SharedPtr client_ptr; + + void send_goal() + { + auto goal_msg = move_action::Goal(); + goal_msg.x_pos = 10.0; // For demonstration purposes + goal_msg.y_pos = 7.5; + + RCLCPP_INFO(this->get_logger(), "Sending goal to x: %f, y: %f", goal_msg.x_pos, goal_msg.y_pos); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.goal_response_callback = + std::bind(&MoveActionClient::goal_response_callback, this, std::placeholders::_1); + + send_goal_options.feedback_callback = + std::bind(&MoveActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + + send_goal_options.result_callback = + std::bind(&MoveActionClient::result_callback, this, std::placeholders::_1); + + this->client_ptr->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( + goal_handle_move::SharedPtr, + const std::shared_ptr feedback) + { + RCLCPP_INFO(this->get_logger(), "Distance to goal: %f", feedback->distance_to_goal); + } + + void result_callback(const goal_handle_move::WrappedResult &result) + { + switch(result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(this->get_logger(), "Goal succeeded! Time taken: %f", result.result->time_taken); + 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; + } + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto client_node = std::make_shared(); + rclcpp::spin(client_node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/challenge_package/src/move_action_server.cpp b/src/challenge_package/src/move_action_server.cpp new file mode 100644 index 0000000..c1bf555 --- /dev/null +++ b/src/challenge_package/src/move_action_server.cpp @@ -0,0 +1,136 @@ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include +#include + +#include "challenge_package/common.hpp" +#include "challenge_package/action/move.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +class MoveActionServer : public rclcpp::Node +{ + using twist_msg = geometry_msgs::msg::Twist; + using pose_msg = turtlesim::msg::Pose; + using move_action = challenge_package::action::Move; + using goal_handle_move = rclcpp_action::ServerGoalHandle; + + + public: + MoveActionServer() + : Node("move_action_service") + { + vel_pub = create_publisher("turtle1/cmd_vel", 10); + pose_sub = create_subscription("turtle1/pose", 10, std::bind(&MoveActionServer::get_pose, this, _1)); + action_server = rclcpp_action::create_server(this, "move_action", + std::bind(&MoveActionServer::handle_goal, this, _1, _2), + std::bind(&MoveActionServer::handle_cancel, this, _1), + std::bind(&MoveActionServer::handle_accepted, this, _1) + ); + } + + private: + Position curr_pos; + Position goal_pos; + rclcpp::Publisher::SharedPtr vel_pub; + rclcpp::Subscription::SharedPtr pose_sub; + rclcpp_action::Server::SharedPtr action_server; + + float get_euc_dist() { + double x = std::pow((goal_pos.x - curr_pos.x), 2); + double y = std::pow((goal_pos.y - curr_pos.y), 2); + double dist = std::sqrt(x+y); + RCLCPP_DEBUG(this->get_logger(), "X Dist: %f, Y Dist: %f, Total Dist: %f", x, y, dist); + return dist; + } + + void get_pose(const pose_msg::SharedPtr msg) { + curr_pos.x = msg->x; + curr_pos.y = msg->y; + } + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Received goal request with x: %f, y: %f", goal->x_pos, goal->y_pos); + (void)uuid; + goal_pos.x = goal->x_pos; + goal_pos.y = goal->y_pos; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse 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 handle_accepted(const std::shared_ptr goal_handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&MoveActionServer::execute, this, _1), goal_handle}.detach(); + } + + void execute(const std::shared_ptr goal_handle) { + rclcpp::Time start_time = this->now(); + RCLCPP_INFO(this->get_logger(), "Executing goal request with x: %f, y: %f", goal_pos.x, goal_pos.y); + + rclcpp::Rate action_rate(1); + auto feedback_ptr = std::make_shared(); + auto result_ptr = std::make_shared(); + + while (rclcpp::ok() && get_euc_dist() > 0.1) { + if (goal_handle->is_canceling()) { + rclcpp::Time end_time = this->now(); + result_ptr->time_taken = (end_time-start_time).seconds(); + goal_handle->canceled(result_ptr); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + + + auto move_msg = twist_msg(); + move_msg.linear.x = (goal_pos.x - curr_pos.x)/2; + move_msg.linear.y = (goal_pos.y - curr_pos.y)/2; + move_msg.linear.z = 0; + move_msg.angular.x = 0; + move_msg.angular.y = 0; + move_msg.angular.z = 0; + + vel_pub->publish(move_msg); + + + feedback_ptr->distance_to_goal = get_euc_dist();; + goal_handle->publish_feedback(feedback_ptr); + // stall the main action loop before continuing + action_rate.sleep(); + } + + // Check if goal is done + if (rclcpp::ok()) { + rclcpp::Time end_time = this->now(); + result_ptr->time_taken = (end_time-start_time).seconds(); + auto move_msg = twist_msg(); + move_msg.linear.x = 0; + move_msg.linear.y = 0; + move_msg.linear.z = 0; + move_msg.angular.x = 0; + move_msg.angular.y = 0; + move_msg.angular.z = 0; + vel_pub->publish(move_msg); + goal_handle->succeed(result_ptr); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/challenge_package/src/move_circle.cpp b/src/challenge_package/src/move_circle.cpp new file mode 100644 index 0000000..05bf26b --- /dev/null +++ b/src/challenge_package/src/move_circle.cpp @@ -0,0 +1,76 @@ +// Build a service for this task +// Service? Basically a gateway before a request goes to the node service + +// Has a request and response? + +#include "rclcpp/rclcpp.hpp" +#include //need to import specific hpp, then specifically refer to it +#include +#include + +// best practice: +// ros1 does not have components (so youd put them in namespaces - used to organize classes and definitions) +// generally always define node as a class (so I could do with just creating a class as an rclcpp node?) +using std::placeholders::_1; +using std::placeholders::_2; +using namespace std::chrono_literals; +// need to use chrono_literals to use things like "500ms" + + +class MoveTurtleCircularService : public rclcpp::Node +{ + // Why have these imports? Will find out later + // Why trigger to define trigger service? + using trigger_srv = std_srvs::srv::Trigger; + using twist_msg = geometry_msgs::msg::Twist; + + + // trigger services -> template for service + public: + MoveTurtleCircularService(): Node("circular_motion_node") { // why does this need to be the same name? + move_service = create_service("move_turtle1_circular", std::bind(&MoveTurtleCircularService::handle_move_req, this, _1, _2)); + move_publisher = create_publisher("turtle1/cmd_vel", 10); + } + + private: // difference between using std::shared_ptr + + // NEED TO SET THESE IN THE PRIVATE FROM PUBLIC + rclcpp::Service::SharedPtr move_service; + rclcpp::Publisher::SharedPtr move_publisher; + rclcpp::TimerBase::SharedPtr move_timer; + + void handle_move_req( + const std::shared_ptr request, std::shared_ptr response + ) // do i even need the request and response if they actually arent used? Yes as its the trigger type + { + RCLCPP_INFO(get_logger(), "Receieved request, moving turtle"); + move_timer = create_wall_timer(500ms, std::bind(&MoveTurtleCircularService::publish_move_turtle_msg, this)); + // logic to move turtle + // to move turtle in a circle, what needs to happen? + // periodically move a turtle right after x seconds + + + response->success = true; + } + + void publish_move_turtle_msg() { + // Whats the difference between linear and angular? A: https://stackoverflow.com/questions/50976281/what-do-x-y-and-z-mean-in-geometry-msgs-twist-message-in-ros + auto circle_msg = twist_msg(); + // circle_msg.linear.x = 1; + circle_msg.linear.y = 1; + // circle_msg.linear.z = 1; // doesnt matter cuz turtle only in 2D + circle_msg.angular.x = 0; + circle_msg.angular.y = 0; + circle_msg.angular.z = 1; // spins around the z-axis, which would be in x and y. + move_publisher->publish(circle_msg); + } + + +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/src/challenge_package/src/publisher_dist.cpp b/src/challenge_package/src/publisher_dist.cpp new file mode 100644 index 0000000..02ec42e --- /dev/null +++ b/src/challenge_package/src/publisher_dist.cpp @@ -0,0 +1,83 @@ +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +using std::placeholders::_1; +using namespace std::chrono_literals; + +struct Position { + float x; + float y; + float theta; +}; + +class DistancePublisher : public rclcpp::Node +{ + using twist_msg = geometry_msgs::msg::Twist; + using pose_msg = turtlesim::msg::Pose; + Position moving_turtle_pos; + Position static_turtle_pos; + + // trigger services -> template for service + public: + DistancePublisher(): Node("distance_publisher_node") { // why does this need to be the same + rclcpp::SubscriptionOptions callback_options; + callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_options.callback_group = callback_group; + + //https://docs.ros.org/en/foxy/How-To-Guides/Using-callback-groups.html + pos_subscriber_stationary = create_subscription( + "/stationary_turtle/pose", + 10, + std::bind(&DistancePublisher::get_stat_pos, this, _1), + callback_options); + + + pos_subscriber_move = create_subscription( + "/moving_turtle/pose", + 10, + std::bind(&DistancePublisher::get_move_pos, this, _1), + callback_options); + + dist_publisher = create_publisher("/turtle_distances", 20); + timer = this->create_wall_timer( + 5000ms, std::bind(&DistancePublisher::get_dist, this)); + } + + private: + rclcpp::Subscription::SharedPtr pos_subscriber_stationary; + rclcpp::Subscription::SharedPtr pos_subscriber_move; + rclcpp::Publisher::SharedPtr dist_publisher; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::TimerBase::SharedPtr timer; + // rclcpp::TimerBase::SharedPtr move_timer; + + void get_move_pos(const pose_msg & msg) { + moving_turtle_pos.x = msg.x; + moving_turtle_pos.y = msg.y; + RCLCPP_INFO(this->get_logger(), "moving_x: %f, moving_y: %f", moving_turtle_pos.x, moving_turtle_pos.y); + } + + void get_stat_pos(const pose_msg & msg) { + static_turtle_pos.x = msg.x; + static_turtle_pos.y = msg.y; + RCLCPP_INFO(this->get_logger(), "static_x: %f, static_y: %f", static_turtle_pos.x, static_turtle_pos.y); + } + + void get_dist() { + double x = std::pow((moving_turtle_pos.x - static_turtle_pos.x), 2); + double y = std::pow((moving_turtle_pos.y - static_turtle_pos.y), 2); + double dist = std::sqrt(x+y); + RCLCPP_INFO(this->get_logger(), "X Dist: %f, Y Dist: %f, Total Dist: %f", x, y, dist); + dist_publisher->publish(dist); + } + +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + // rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/src/challenge_package/src/reset_turtle.cpp b/src/challenge_package/src/reset_turtle.cpp new file mode 100644 index 0000000..dba1826 --- /dev/null +++ b/src/challenge_package/src/reset_turtle.cpp @@ -0,0 +1,59 @@ +// Also a service +#include "rclcpp/rclcpp.hpp" +#include //need to import specific hpp, then specifically refer to it +#include "turtlesim/srv/teleport_absolute.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +// move these to a central file and import +struct Position { + float x; + float y; + float theta; +}; + +struct Turtle { + _Float32 x; + _Float32 y; + _Float32 theta; + std::string name; +}; +class ResetTurtleService : public rclcpp::Node +{ + using trigger_srv = std_srvs::srv::Trigger; + + public: + ResetTurtleService(): Node("reset_turtle_node") { + reset_service = create_service("reset_moving_turtle", std::bind(&ResetTurtleService::handle_reset_req, this, _1, _2)); + turtlesim_client = create_client("/moving_turtle/teleport_absolute"); // client to call specific route for the turtlesim service + } + + private: + rclcpp::Service::SharedPtr reset_service; + rclcpp::Client::SharedPtr turtlesim_client; + Position init_pos = {5,5,0}; + + void handle_reset_req(const std::shared_ptr request, std::shared_ptr response) + { + RCLCPP_INFO(get_logger(), "Receieved request, resetting turtle"); + // spawn turtles + + auto reset_req = std::make_shared(); + reset_req->x = init_pos.x; + reset_req->y = init_pos.y; + reset_req->theta = init_pos.theta; + auto res = turtlesim_client->async_send_request(reset_req); + response->success = true; + response->message = "Position reset"; + return; + } + +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/src/challenge_package/src/spawn_turtles.cpp b/src/challenge_package/src/spawn_turtles.cpp new file mode 100644 index 0000000..833e834 --- /dev/null +++ b/src/challenge_package/src/spawn_turtles.cpp @@ -0,0 +1,60 @@ +// Also a service +#include "rclcpp/rclcpp.hpp" +#include //need to import specific hpp, then specifically refer to it +#include "turtlesim/srv/spawn.hpp" + +using std::placeholders::_1; +using std::placeholders::_2; + +class SpawnTurtleService : public rclcpp::Node +{ + using trigger_srv = std_srvs::srv::Trigger; + + public: + SpawnTurtleService(): Node("spawn_turtle_node") { + spawn_service = create_service("spawn_turtles", std::bind(&SpawnTurtleService::handle_spawn_req, this, _1, _2)); + turtlesim_client = create_client("/spawn"); + } + + private: + rclcpp::Service::SharedPtr spawn_service; + rclcpp::Client::SharedPtr turtlesim_client; + + struct Turtle { + _Float32 x; + _Float32 y; + _Float32 theta; + std::string name; + }; + + //Spawns a turtle named "stationary_turtle" at x = 5, y = 5 Spawns a second turtle named "moving_turtle" at x = 25, y = 10 + + void handle_spawn_req(const std::shared_ptr request, std::shared_ptr response) + { + // Establish data structure with 2 turtles + std::vector turtles = {{5,5,0,"stationary_turtle"}, {25,10,0,"moving_turtle"}}; + RCLCPP_INFO(get_logger(), "Receieved request, moving turtle"); + // spawn turtles + + for (unsigned int i = 0; i < turtles.size(); i++) { + auto spawn_req = std::make_shared(); + spawn_req->x = turtles[i].x; + spawn_req->y = turtles[i].y; + spawn_req->theta = turtles[i].theta; + spawn_req->name = turtles[i].name; + auto res = turtlesim_client->async_send_request(spawn_req); + + RCLCPP_INFO(this->get_logger(), "Spawned Turtle: '%s'", spawn_req->name.c_str()); + } + + response->success = true; + } + +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} \ No newline at end of file