diff --git a/.gitignore b/.gitignore index 40f0bf4..5271af0 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,67 @@ -/software_training/launch/__pycache__/ \ No newline at end of file + +/software_training/launch/__pycache__/ + +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE + +.vscode/ + +.idea/ + +CMakeLists.txt.user + +cmake-build-debug/ + +install/ + + +log/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9b12e3c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,131 @@ +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() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(turtlesim REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + + +#include the 'include' directory +include_directories(include) + +# add clear_turtles_node +add_library(turtle_request SHARED + src/clear_turtles_node.cpp) + +target_compile_definitions(turtle_request PRIVATE "COMPOSITION_BUILDING_DLL") + +ament_target_dependencies(turtle_request +"rclcpp" +"rclcpp_components" +"turtlesim" +"std_msgs") +rclcpp_components_register_nodes(turtle_request "composition::turtle_service_request_node") + +#add reset_turtle_node +add_library(turtle_reset_request SHARED + src/reset_turtle_node.cpp) + +ament_target_dependencies(turtle_reset_request +"rclcpp" +"rclcpp_components" +"turtlesim" +"std_msgs") +rclcpp_components_register_nodes(turtle_reset_request "composition::turtle_service_request_node") + + +rosidl_generate_interfaces(${PROJECT_NAME} +"msg/Software.msg" +"srv/Software.srv" +"action/Software.action" + DEPENDENCIES std_msgs geometry_msgs builtin_interfaces +) +ament_export_dependencies(rosidl_default_runtime) + + +add_library(circular_turtle_request SHARED + src/circular_turtle_node.cpp) +ament_target_dependencies(circular_turtle_request +"rclcpp" +"rclcpp_components" +"turtlesim" +"std_msgs" +"geometry_msgs") +rclcpp_components_register_nodes(circular_turtle_request "composition::circular_turtle_node") + + +add_library(spawn_turtle_request SHARED + src/spawn_turtles_node.cpp) +ament_target_dependencies(spawn_turtle_request +"rclcpp" +"rclcpp_components" +"turtlesim" +"std_msgs") +rclcpp_components_register_nodes(circular_turtle_request "composition::spawn_turtles_node") + + +add_library(turtle_distance_request SHARED + src/turtle_distance_node.cpp) +ament_target_dependencies(turtle_distance_request +"rclcpp" +"rclcpp_components" +"turtlesim" +"std_msgs" +"geometry_msgs") +rclcpp_components_register_nodes(turtle_distance_request "composition::turtle_distance_node") + +add_library(moving_turtle_action_node SHARED + src/moving_turtle_action_node.cpp) +ament_target_dependencies(moving_turtle_action_node +"rclcpp" +"rclcpp_components" +"turtlesim" +"std_msgs" +"geometry_msgs") +rclcpp_components_register_nodes(moving_turtle_action_node "composition::moving_turtle_action_node") + + +install(TARGETS + turtle_request + turtle_reset_request + circular_turtle_request + turtle_distance_request + spawn_turtle_request + 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/software_training/action/Software.action b/action/Moving.action similarity index 92% rename from software_training/action/Software.action rename to action/Moving.action index ac77e1c..26889d4 100644 --- a/software_training/action/Software.action +++ b/action/Moving.action @@ -1,16 +1,14 @@ + # Goal Request /moving_turtle/cmd_vel geometry_msgs/Vector3 linear_pos geometry_msgs/Vector3 angular_pos # Result - time --- -uint64 duration +int64 duration --- # Feedback /moving_turtle/pose float32 x_pos float32 y_pos float32 theta_pos - - - diff --git a/include/software_training_assignment/circular_turtle_node.hpp b/include/software_training_assignment/circular_turtle_node.hpp new file mode 100644 index 0000000..3ae5de5 --- /dev/null +++ b/include/software_training_assignment/circular_turtle_node.hpp @@ -0,0 +1,36 @@ +//ppdirective that states the header only gets included once in the project + +#pragma once + +#include +#include +namespace composition{ + class circular_turtle : public rclcpp::Node { + public: + circular_turtle(const rclcpp::NodeOptions &options); + private: + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher; + //need to figure out how to publish a message to turtle1 outlining its angular velocity and all that + typedef struct point { + + typedef struct linear { + static constexpr float x = 12; + static constexpr float y = 12; + static constexpr float z = 12; + } linear; + + typedef struct angular { + static constexpr float x = 1.41; + static constexpr float y = 1.41; + static constexpr float z = 1.41; + + } angular; + + } coordinates; + void rotate(); + +}; +} + + diff --git a/include/software_training_assignment/clear_turtles_node.hpp b/include/software_training_assignment/clear_turtles_node.hpp new file mode 100644 index 0000000..08e9eae --- /dev/null +++ b/include/software_training_assignment/clear_turtles_node.hpp @@ -0,0 +1,24 @@ +#pragma once + +//get all turtle names and request to kill each one +//make a request to the turtle killing service + +#include +#include +#include + + + +namespace composition { +class clear_turtles_node : public rclcpp::Node{ + public: + clear_turtles_node(const rclcpp::NodeOptions &options); + + private: + rclcpp::Client::SharedPtr client; //shared request + std::vector nodes = {"turtle1", "moving_turtle", "stationary_turtle"}; + rclcpp::TimerBase::SharedPtr timer_; + void kill(); + +}; +} diff --git a/include/software_training_assignment/moving_turtle_action_node.hpp b/include/software_training_assignment/moving_turtle_action_node.hpp new file mode 100644 index 0000000..989f0c7 --- /dev/null +++ b/include/software_training_assignment/moving_turtle_action_node.hpp @@ -0,0 +1,37 @@ +#include +#include +#include +#include +#include +#include + +namespace composition{ +class moving_turtle_action_node : public rclcpp::Node { + public: + + moving_turtle_action_node(); + using GoalHandleActionServer = + rclcpp_action::ServerGoalHandle; + private: + float x; + float y; + float theta; + + rclcpp_action::Server::SharedPtr action_server; + + rclcpp::Publisher::SharedPtr publisher; + + rclcpp::Subscription::SharedPtr subscriber; + + //copied some code from software training, since action server is hard to figure out + 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); + + static constexpr unsigned int QUEUE{10}; + +}; +} diff --git a/include/software_training_assignment/reset_turtle_node.hpp b/include/software_training_assignment/reset_turtle_node.hpp new file mode 100644 index 0000000..d57cdcb --- /dev/null +++ b/include/software_training_assignment/reset_turtle_node.hpp @@ -0,0 +1,20 @@ +#pragma once +#include +#include +#include +// #include + +namespace composition{ + class reset_turtle_node : public rclcpp::Node { + public: + reset_turtle_node(const rclcpp::NodeOptions &options); + private: + //make a service.... + //how do i do that lmao + rclcpp::Service::SharedPtr service_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Client::SharedPtr client_; + void reset(); + void reset_moving_turtle(); + }; +} \ No newline at end of file diff --git a/include/software_training_assignment/spawn_turtles_node.hpp b/include/software_training_assignment/spawn_turtles_node.hpp new file mode 100644 index 0000000..da55daa --- /dev/null +++ b/include/software_training_assignment/spawn_turtles_node.hpp @@ -0,0 +1,17 @@ +#include +#include +#include + +class spawn_turtles : public rclcpp::Node { +private: + /* data */ + rclcpp::Client::SharedPtr client; + + rclcpp::TimerBase::SharedPtr timer_; + void spawn(); + +public: + spawn_turtles(const rclcpp::NodeOptions &options); + +}; + diff --git a/include/software_training_assignment/turtle_distance_node.hpp b/include/software_training_assignment/turtle_distance_node.hpp new file mode 100644 index 0000000..b55c5ec --- /dev/null +++ b/include/software_training_assignment/turtle_distance_node.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include +#include + +namespace composition { +class turtle_distance_node : public rclcpp::Node { + public: + turtle_distance_node(const rclcpp::NodeOptions &options); + private: + rclcpp::Subscription::SharedPtr subscription; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; + float turtleDistance(); + + float turtle1_x; + float turtle1_y; + float turtle2_x; + float turtle2_y; +}; +} + diff --git a/msg/Software.msg b/msg/Software.msg new file mode 100644 index 0000000..c685ffe --- /dev/null +++ b/msg/Software.msg @@ -0,0 +1,3 @@ +float64 x +float64 y +float64 distance \ No newline at end of file diff --git a/software_training/package.xml b/package.xml similarity index 60% rename from software_training/package.xml rename to package.xml index 35fd6b4..67e8bd6 100644 --- a/software_training/package.xml +++ b/package.xml @@ -1,35 +1,22 @@ - software_training + software_training_assignment 0.0.0 TODO: Package description - niiquaye + andrew TODO: License declaration ament_cmake - - - rclcpp_components rclcpp - std_msgs turtlesim - rcutils - geometry_msgs - builtin_interfaces rosidl_default_generators - rclcpp_action - + rosidl_default_generators + rclcpp_components + std_msgs - rclcpp - rclcpp_components - launch_ros - std_msgs - rcutils - turtlesim - geometry_msgs rosidl_default_runtime - rclcpp_action + rclcpp ament_lint_auto ament_lint_common diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt deleted file mode 100644 index e93eafd..0000000 --- a/software_training/CMakeLists.txt +++ /dev/null @@ -1,183 +0,0 @@ -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(turtlesim REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs 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) - - -#include the 'include' directory -include_directories(include) - -# custom services and messages and actions -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Software.msg" - "srv/Software.srv" - "action/Software.action" - DEPENDENCIES std_msgs geometry_msgs builtin_interfaces - ) -ament_export_dependencies(rosidl_default_runtime) - -#create resource which references the libraries in the binary bin -set(node_plugins "") - -#add plugins as SHARED library - -#add turtle_request_service_node as a plugin -add_library(turtle_request SHARED - src/turtle_neutralize.cpp) -target_compile_definitions(turtle_request - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(turtle_request - "rclcpp" - "rclcpp_components" - "turtlesim" - "std_msgs") -rclcpp_components_register_nodes(turtle_request "composition::turtle_service_request_node") -# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node -set(node_plugins "${node_plugins}compositon::turtle_service_request_node;$\n") - - -#add spawn_turtle_nodelets as plugin -add_library(turtle_spawn SHARED - src/spawn_turtle_nodelet.cpp) -target_compile_definitions(turtle_spawn - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(turtle_spawn - "rclcpp" - "rclcpp_components" - "turtlesim" - "std_msgs") -rclcpp_components_register_nodes(turtle_spawn "composition::spawn_turtle_nodelet") -set(node_plugins "${node_plugins}composition::spawn_turtle_nodelet;$") - - -#add turtle publisher nodelet -add_library(turtle_pub SHARED - src/turtle_publisher.cpp) -target_compile_definitions(turtle_pub - PRIVATE "COMPOSITION_BUILDING_DLL" - ) -ament_target_dependencies(turtle_pub - "rclcpp" - "rclcpp_components" - "turtlesim" - "std_msgs") -rosidl_target_interfaces(turtle_pub ${PROJECT_NAME} "rosidl_typesupport_cpp") # need this for custom messages - idk why tho -rclcpp_components_register_nodes(turtle_pub "composition::turtle_publisher") -set(node_plugins "${node_plugins}composition::turtle_publisher;$") - -#add moving turtle service -add_library(turtle_service SHARED - src/reset_moving_turtle_service.cpp) -target_compile_definitions(turtle_service - PRIVATE "COMPOSITION_BUILDING_DLL" - ) -ament_target_dependencies(turtle_service - "rclcpp" - "rclcpp_components" - "turtlesim" - "turtlesim" - "std_msgs") -rosidl_target_interfaces(turtle_service ${PROJECT_NAME} "rosidl_typesupport_cpp") # need for custom srv -rclcpp_components_register_nodes(turtle_service "composition::reset_moving_turtle_service") -set(node_plugins "${node_plugins}composition::reset_moving_turtle_service;$) - -#add cmd_vel publisher for moving_turtle -add_library(cmd_vel_publisher SHARED - src/cmd_vel_moving_turt_publisher.cpp) -target_compile_definitions(cmd_vel_publisher - PRIVATE "COMPOSITION_BUILDING_DLL" - ) -ament_target_dependencies(cmd_vel_publisher - "rclcpp" - "rclcpp_components" - "turtlesim" - "geometry_msgs" - "std_msgs") -rclcpp_components_register_nodes(cmd_vel_publisher "composition::cmd_vel_moving_turt_publisher") -set(node_plugins "${node_plugins}composition::cmd_vel_moving_turt_publisher;$) - -#add action server for moving turtle -#add_library(moving_turt_action SHARED -# src/moving_turtle_action_server.cpp) -#target_compile_definitions(moving_turt_action -# PRIVATE "COMPOSITION_BUILDING_DLL" -# ) -#ament_target_dependencies(moving_turt_action -# "rclcpp" -# "rclcpp_components" -# "turtlesim" -# "rclcpp_action" -# "geometry_msgs" -# "std_msgs") -#rosidl_target_interfaces(moving_turt_action ${PROJECT_NAME} "rosidl_typesupport_cpp") # needed for custom action -#rclcpp_components_register_nodes(moving_turt_action "composition::moving_turtle_action_server") -#set(node_plugins "${node_plugins}composition::moving_turt_action_server;$") - -add_library(turtle_action_server SHARED - src/moving_turtle_action_server.cpp) -target_compile_definitions(turtle_action_server - PRIVATE "SOFTWARE_TRAINING_DLL") -ament_target_dependencies(turtle_action_server - "rclcpp" - "rclcpp_components" - "turtlesim" - "rclcpp_action" - "std_msgs" - "geometry_msgs") -rosidl_target_interfaces(turtle_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp") # needed for custom action -# with this we can exexute the component as a node - ros2 run software_training moving_action_server -rclcpp_components_register_node(turtle_action_server PLUGIN "composition::moving_turtle_action_server" EXECUTABLE moving_action_server) - - - -#tell where to put binaries -install(TARGETS - turtle_request - turtle_spawn - turtle_pub - turtle_service - #moving_turt_action - turtle_action_server - cmd_vel_publisher - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -#install launch file -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} - ) - -#install configuration file (yaml files) for param server -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME} - ) - -ament_package() diff --git a/software_training/include/software_training/cmd_vel_moving_turt_publisher.hpp b/software_training/include/software_training/cmd_vel_moving_turt_publisher.hpp deleted file mode 100644 index 01e1806..0000000 --- a/software_training/include/software_training/cmd_vel_moving_turt_publisher.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef CMD_VEL_MOVING_TURT_PUBLISHER_HPP_ -#define CMD_VEL_MOVING_TURT_PUBLISHER_HPP_ - -#include -#include -#include -#include - -#include - -#include -#include - -namespace composition { - -class cmd_vel_moving_turt_publisher : public rclcpp::Node { - -public: - SOFTWARE_TRAINING_PUBLIC - explicit cmd_vel_moving_turt_publisher(const rclcpp::NodeOptions &options); - -private: - // publisher - rclcpp::Publisher::SharedPtr publisher; - - // callback timer - rclcpp::TimerBase::SharedPtr timer; - - // set quality of service depth - AKA a backlog - static constexpr unsigned int QUEUE{10}; - - // namespace for values - typedef struct point { - - typedef struct linear { - static constexpr float x = 12; - static constexpr float y = 12; - static constexpr float z = 12; - } linear; - - typedef struct angular { - static constexpr float x = 1.41; - static constexpr float y = 1.41; - static constexpr float z = 1.41; - - } angular; - - } coordinates; -}; - -} // namespace composition - -#endif // CMD_VEL_MOVING_TURT_PUBLISHER_HPP_ diff --git a/software_training/include/software_training/moving_turtle_action_server.hpp b/software_training/include/software_training/moving_turtle_action_server.hpp deleted file mode 100644 index 43837cc..0000000 --- a/software_training/include/software_training/moving_turtle_action_server.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef MOVING_TURTLE_ACTION_SERVER_HPP_ -#define MOVING_TURTLE_ACTION_SERVER_HPP_ - -#include -#include -#include -#include - -#include -#include // ros2 time header -#include // ros2 action header -#include -#include - -#include // cmd_vel publisher message -#include // header for message to get moving turt position - -namespace composition{ - -class moving_turtle_action_server : public rclcpp::Node { - -public: - SOFTWARE_TRAINING_PUBLIC - explicit moving_turtle_action_server(const rclcpp::NodeOptions &options); - - // nice to have to prevent lengthy repitive code - using GoalHandleActionServer = - rclcpp_action::ServerGoalHandle; - -private: - // action server - rclcpp_action::Server::SharedPtr - action_server; - - // publisher to publish moving turtle commands - rclcpp::Publisher::SharedPtr publisher; - - // subscriber to get moving turt posiiton - rclcpp::Subscription::SharedPtr subscriber; - - // goal callback function - SOFTWARE_TRAINING_LOCAL - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &uuid, - std::shared_ptr goal); - - // cancel response callback function - SOFTWARE_TRAINING_LOCAL - rclcpp_action::CancelResponse - handle_cancel(const std::shared_ptr goal_handle); - - // handle accepetd callback function - SOFTWARE_TRAINING_LOCAL - void - handle_accepted(const std::shared_ptr goal_handle); - - // executioner callback function - void execute(const std::shared_ptr goal_handle); - - // for subscriber - static float x; - static float y; - static float theta; - static float linear_velocity; - static float angular_velocity; - - static constexpr unsigned int QUEUE{10}; -}; - -} - -#endif // MOVING_TURTLE_ACTION_SERVER_HPP_ diff --git a/software_training/include/software_training/reset_moving_turtle_service.hpp b/software_training/include/software_training/reset_moving_turtle_service.hpp deleted file mode 100644 index 4c6cf1b..0000000 --- a/software_training/include/software_training/reset_moving_turtle_service.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef RESET_MOVING_TURTLE_SERVICE_HPP_ -#define RESET_MOVING_TURTLE_SERVICE_HPP_ - -#include -#include -#include - -#include - -#include -#include -#include - -namespace composition { - -class reset_moving_turtle_service : public rclcpp::Node { - -public: - SOFTWARE_TRAINING_PUBLIC - explicit reset_moving_turtle_service(const rclcpp::NodeOptions &options); - -private: - // create service that will reset turtle to starting point - rclcpp::Service::SharedPtr service; - - // create client - rclcpp::Client::SharedPtr client; - - // server callback - SOFTWARE_TRAINING_LOCAL - void service_callback( - const std::shared_ptr request, - std::shared_ptr response); - - typedef struct reset { - constexpr static float x = 5.44; - constexpr static float y = 5.44; - constexpr static float theta = 0; - } reset_coordinates; -}; - -} // namespace composition - -#endif // RESET_MOVING_TURTLE_SERVICE_HPP_ diff --git a/software_training/include/software_training/spawn_turtle_nodelet.hpp b/software_training/include/software_training/spawn_turtle_nodelet.hpp deleted file mode 100644 index 7a0f918..0000000 --- a/software_training/include/software_training/spawn_turtle_nodelet.hpp +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef SPAWN_TURTLE_NODELET_HPP_ -#define SPAWN_TURTLE_NODELET_HPP_ - -#include -#include -#include -#include -#include - -#include - -#include -#include - -namespace composition { - -class spawn_turtle_nodelet : public rclcpp::Node { - -public: - SOFTWARE_TRAINING_PUBLIC - explicit spawn_turtle_nodelet(const rclcpp::NodeOptions &options); - -private: - rclcpp::Client::SharedPtr client; - rclcpp::TimerBase::SharedPtr timer; - - SOFTWARE_TRAINING_LOCAL - void spawn_turtle(); - - static const unsigned int NUMBER_OF_TURTLES{2}; - - typedef struct turtle_info { - float x_pos; - float y_pos; - float rad; - } turtle_info; - - std::vector turtle_names{"stationary_turtle", "moving_turtle"}; - std::vector turtle_bio{{.x_pos = 5, .y_pos = 5, .rad = 0}, - {.x_pos = 25, .y_pos = 10, .rad = 0}}; - - // map of turtle name to turtle information - std::map turtle_description; -}; - -} // namespace composition - -#endif // SPAWN_TURTLE_NODELET_HPP_ diff --git a/software_training/include/software_training/turtle_neutralize.hpp b/software_training/include/software_training/turtle_neutralize.hpp deleted file mode 100644 index ea09a63..0000000 --- a/software_training/include/software_training/turtle_neutralize.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ -#define TURTLE_SERVICE_REQUEST_NODE_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_ diff --git a/software_training/include/software_training/turtle_publisher.hpp b/software_training/include/software_training/turtle_publisher.hpp deleted file mode 100644 index 4aeb674..0000000 --- a/software_training/include/software_training/turtle_publisher.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef TURTLE_PUBLISHER_HPP_ -#define TURTLE_PUBLISHER_HPP_ - -#include - -#include - -#include -#include -#include - -namespace composition { - -class turtle_publisher : public rclcpp::Node { -public: - SOFTWARE_TRAINING_PUBLIC - explicit turtle_publisher(const rclcpp::NodeOptions &options); - -private: - // position subscribers - rclcpp::Subscription::SharedPtr stationary_turt_sub; - rclcpp::Subscription::SharedPtr moving_turt_sub; - - // turtle publisher with custom message - rclcpp::Publisher::SharedPtr publisher; - - // timer for publisher callback - rclcpp::TimerBase::SharedPtr timer; - - // callback groups - really just threads to run callbacks - rclcpp::CallbackGroup::SharedPtr callbacks; - - float x_stationary_turt; - float y_stationary_turt; - - float x_moving_turt; - float y_moving_turt; - - float total_distance; - - static const unsigned int QUEUE{10}; -}; -} // namespace composition - -#endif // TURTLE_PUBLISHER_HPP_ diff --git a/software_training/include/software_training/visibility.h b/software_training/include/software_training/visibility.h deleted file mode 100644 index 7465546..0000000 --- a/software_training/include/software_training/visibility.h +++ /dev/null @@ -1,65 +0,0 @@ -// 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_ diff --git a/software_training/launch/software_training.launch.py b/software_training/launch/software_training.launch.py deleted file mode 100644 index dbbcbca..0000000 --- a/software_training/launch/software_training.launch.py +++ /dev/null @@ -1,21 +0,0 @@ -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -def generate_launch_description(): - """Generate launch description with multiple components.""" - container = ComposableNodeContainer( - name='my_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='software_training', - plugin='composition::turtle_service_request_node', - name='turtle_request'), - ], - output='screen', - ) - - return launch.LaunchDescription([container]) diff --git a/software_training/msg/Software.msg b/software_training/msg/Software.msg deleted file mode 100644 index 8dd0f54..0000000 --- a/software_training/msg/Software.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 x_pos -float64 y_pos -float64 distance diff --git a/software_training/src/cmd_vel_moving_turt_publisher.cpp b/software_training/src/cmd_vel_moving_turt_publisher.cpp deleted file mode 100644 index 4fbbada..0000000 --- a/software_training/src/cmd_vel_moving_turt_publisher.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include - -using namespace std::chrono_literals; - -namespace composition { - -cmd_vel_moving_turt_publisher::cmd_vel_moving_turt_publisher( - const rclcpp::NodeOptions &options) - : Node("cmd_vel_moving_turt_publisher", options) { - - auto publisher_callback = [this](void) -> void { - auto message = std::make_unique(); - message->linear.x = cmd_vel_moving_turt_publisher::coordinates::linear::x; - message->linear.y = cmd_vel_moving_turt_publisher::coordinates::linear::y; - message->linear.z = cmd_vel_moving_turt_publisher::coordinates::linear::z; - - message->angular.x = cmd_vel_moving_turt_publisher::coordinates::angular::x; - message->angular.y = cmd_vel_moving_turt_publisher::coordinates::angular::y; - message->angular.z = cmd_vel_moving_turt_publisher::coordinates::angular::z; - - this->publisher->publish(std::move(message)); - }; - - // create publisher - // create a Quality of Service object with a history/ depth of 10 calls - this->publisher = this->create_publisher( - "/moving_turtle/cmd_vel", rclcpp::QoS(QUEUE)); - - this->timer = this->create_wall_timer(1ms, publisher_callback); -} - -} // namespace composition -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(composition::cmd_vel_moving_turt_publisher) diff --git a/software_training/src/moving_turtle_action_server.cpp b/software_training/src/moving_turtle_action_server.cpp deleted file mode 100644 index 3dc848f..0000000 --- a/software_training/src/moving_turtle_action_server.cpp +++ /dev/null @@ -1,204 +0,0 @@ -#include -#include -#include - -using namespace std::chrono_literals; -using namespace std::placeholders; - -namespace composition { - -moving_turtle_action_server::moving_turtle_action_server( - const rclcpp::NodeOptions &options) - : Node("moving_turtle_action_server", options) { - - // create publisher - this->publisher = this->create_publisher( - "/moving_turtle/cmd_vel", rclcpp::QoS(QUEUE)); - - // TODO: Enable topic statistics for subscriber - - auto subscriber_callback = - [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { - this->moving_turtle_action_server::x = msg->x; - this->moving_turtle_action_server::y = msg->y; - this->moving_turtle_action_server::theta = msg->theta; - this->moving_turtle_action_server::linear_velocity = msg->linear_velocity; - this->moving_turtle_action_server::angular_velocity = msg->angular_velocity; - }; - - // create subscriber - this->subscriber = this->create_subscription( - "/moving_turtle/pose", QUEUE, subscriber_callback); - - // create action server - this->action_server = - rclcpp_action::create_server( - this, "moving_turtle_action_server", - std::bind(&moving_turtle_action_server::handle_goal, this, _1, _2), - std::bind(&moving_turtle_action_server::handle_cancel, this, _1), - std::bind(&moving_turtle_action_server::handle_accepted, this, _1) - - ); -} - -rclcpp_action::GoalResponse moving_turtle_action_server::handle_goal( - const rclcpp_action::GoalUUID &uuid, - std::shared_ptr goal) { - (void)uuid; // not needed - prevents compiler warnings/errors -Wall flag - RCLCPP_INFO(this->get_logger(), "Goal Received"); - RCLCPP_INFO(this->get_logger(), "linear X:%f Y:%f Z:%f", goal->linear_pos.x, - goal->linear_pos.y, goal->linear_pos.z); - RCLCPP_INFO(this->get_logger(), "angular X:%f Y:%f Z:%f", goal->angular_pos.x, - goal->angular_pos.y, goal->angular_pos.z); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse moving_turtle_action_server::handle_cancel( - const std::shared_ptr goal_handle) { - - (void)goal_handle; // not needed - - RCLCPP_INFO(this->get_logger(), "Recieved Request to cancel goal!"); - - return rclcpp_action::CancelResponse::ACCEPT; -} - -void moving_turtle_action_server::handle_accepted( - const std::shared_ptr goal_handle) { - - // accept new goal and then 'execute' goal on a new thread to prevent blocking - // action thus execute the goal on a different thread so no blocking action is - // created detach thread so it can finish on its own - - std::thread{std::bind(&moving_turtle_action_server::execute, this, _1), - goal_handle} - .detach(); -} - -void moving_turtle_action_server::execute( - const std::shared_ptr goal_handle) { - - rclcpp::Time start_time = this->now(); // get the current time - - RCLCPP_INFO(this->get_logger(), "Excuting Goal"); - - rclcpp::Rate cycle_rate{1}; // set up frequency for goal execution - - // acquire the goal to be executed - const auto goal = goal_handle->get_goal(); - - // create feedback - std::unique_ptr feedback = - std::make_unique(); - - // make result - std::unique_ptr result = - std::make_unique(); - - // create reference to feedback for ease of use - float &curr_x = feedback->x_pos; - float &curr_y = feedback->y_pos; - float &curr_theta = feedback->theta_pos; - - // keep track of linear feedback - float lin_x{0}; - float lin_y{0}; - float lin_z{0}; - - // keep track of angular feedback - float ang_x{0}; - float ang_y{0}; - float ang_z{0}; - - // heavy lifting - while (rclcpp::ok() && - (lin_x < goal->linear_pos.x || lin_y < goal->linear_pos.y || - lin_z < goal->linear_pos.z || ang_x < goal->angular_pos.x || - ang_y < goal->angular_pos.y || ang_z < goal->angular_pos.z)) { - - // check if goal has been canceled - if (goal_handle->is_canceling()) { - RCLCPP_INFO(this->get_logger(), "Goal Canceled"); - - // get the time it has taken thus far and update result - rclcpp::Time curr_time = this->now(); - rclcpp::Duration time = curr_time - start_time; - long int duration{time.nanoseconds()}; - result->duration = duration; - - goal_handle->canceled(std::move(result)); - return; - } - - // publish to '/moving_turtle/cmd_vel' topic to move the turtle - - // create message - auto message_cmd_vel = std::make_unique(); - - // fill message contents - // if lin or ang distance points are already at target do not increment any - // more just keep it the same hence the ternary operator - message_cmd_vel->linear.x = (lin_x < goal->linear_pos.x) ? lin_x++ : lin_x; - message_cmd_vel->linear.y = (lin_y < goal->linear_pos.y) ? lin_y++ : lin_y; - message_cmd_vel->linear.z = (lin_z < goal->linear_pos.z) ? lin_z++ : lin_z; - message_cmd_vel->angular.x = - (ang_x < goal->angular_pos.x) ? ang_x++ : ang_x; - message_cmd_vel->angular.y = - (ang_y < goal->angular_pos.y) ? ang_y++ : ang_y; - message_cmd_vel->angular.z = - (ang_z < goal->angular_pos.z) ? ang_z++ : ang_z; - - // publish message - this->publisher->publish(std::move(message_cmd_vel)); - - // now compute feedback - curr_x = this->moving_turtle_action_server::x - lin_x; - curr_y = this->moving_turtle_action_server::y - lin_y; - - /********* MATH TO FIND THETA ************ - * theta = acos(z_axis_component/magnitude of vector) - * - * - ********************************************/ - float theta{0}; - - // scope this stuff cause we dont need it afterwards - { - - float x1{lin_x}, x2{lin_y}, x3{lin_z}; - - float magnitude{static_cast(sqrt((x1 * x1) + (x2 * x2) + (x3 * x3)))}; - - theta = acos(x3 / magnitude); - } - - curr_theta = this->moving_turtle_action_server::theta - theta; - - // publish feedback - goal_handle->publish_feedback(std::move(feedback)); - - cycle_rate.sleep(); // control the rate at which the loop, loops through - } - - // if goal is done - if (rclcpp::ok()) { - - rclcpp::Time end = this->now(); // get end time - rclcpp::Duration duration = end - start_time; // compute time taken - long int res_time{duration.nanoseconds()}; // should be uint64_t - - // fill in result - result->duration = res_time; - - // set the result - goal_handle->succeed( - std::move(result)); // move ownership so ptr is still usable - RCLCPP_INFO(this->get_logger(), "Finish Executing Goal"); - } -} - -} // namespace composition - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(composition::moving_turtle_action_server) diff --git a/software_training/src/reset_moving_turtle_service.cpp b/software_training/src/reset_moving_turtle_service.cpp deleted file mode 100644 index 6b70550..0000000 --- a/software_training/src/reset_moving_turtle_service.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include -#include - -using namespace std::placeholders; -using namespace std::chrono_literals; - -namespace composition { - -reset_moving_turtle_service::reset_moving_turtle_service( - const rclcpp::NodeOptions &options) - : Node("reset_moving_turtle_service", options) { - - // create client - this->client = this->create_client( - "/moving_turtle/teleport_absolute"); - - // create service - this->service = this->create_service( - "/reset_moving_turtle", - std::bind(&reset_moving_turtle_service::service_callback, this, _1, _2)); -} - -void reset_moving_turtle_service::service_callback( - const std::shared_ptr request, - std::shared_ptr response) { - - (void)request; // request is not needed - - RCLCPP_INFO(this->get_logger(), "Starting ..."); - - // make client call to reset turtle - if (!client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "System Aborted"); - response->success = false; - return; - } - RCLCPP_INFO(this->get_logger(), "Service is not available! Exit!"); - response->success = false; - return; - } - - auto client_request = - std::make_shared(); - - // fill request data - client_request->x = reset_moving_turtle_service::reset_coordinates::x; - client_request->y = reset_moving_turtle_service::reset_coordinates::y; - client_request->theta = reset_moving_turtle_service::reset_coordinates::theta; - - // create response callback - auto response_callback = - [this]( - rclcpp::Client::SharedFuture future) - -> void { - (void)future; // not needed - RCLCPP_INFO(this->get_logger(), "Turtle Moved"); - }; - - // send client request - auto result = client->async_send_request(client_request, response_callback); - - RCLCPP_INFO(this->get_logger(), "Turtle Reseted"); - - response->success = true; -} - -} // namespace composition - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(composition::reset_moving_turtle_service) diff --git a/software_training/src/spawn_turtle_nodelet.cpp b/software_training/src/spawn_turtle_nodelet.cpp deleted file mode 100644 index bcbae35..0000000 --- a/software_training/src/spawn_turtle_nodelet.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#include -#include - -using namespace std::chrono_literals; - -namespace composition { - -spawn_turtle_nodelet::spawn_turtle_nodelet(const rclcpp::NodeOptions &options) - : Node("spawn_turtle_nodelet", options) { - - // create client that makes a requets to '/spawn' service - client = this->create_client("/spawn"); - - // create client callback - timer = this->create_wall_timer( - 2s, std::bind(&spawn_turtle_nodelet::spawn_turtle, this)); - - // fill up map with contents - for (size_t i{0}; i < NUMBER_OF_TURTLES; i++) { - turtle_description.insert({turtle_names[i], turtle_bio[i]}); - } -} - -void spawn_turtle_nodelet::spawn_turtle() { - - if (!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"); - return; - } - - for (const std::string &name : turtle_names) { - - // create request - std::unique_ptr request = - std::make_unique(); - - // fill in repsonse - request->name = name; - request->x = turtle_description[name].x_pos; - request->y = turtle_description[name].y_pos; - request->theta = turtle_description[name].rad; - - // create a callback to call client and because no 'spin()' is available - auto callback = - [this](rclcpp::Client::SharedFuture response) - -> void { - RCLCPP_INFO(this->get_logger(), "Turtle Created: %s", - response.get()->name.c_str()); - rclcpp::shutdown(); - }; - - // send request - auto result = client->async_send_request(std::move(request), callback); - } -} - -} // namespace composition - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(composition::spawn_turtle_nodelet) diff --git a/software_training/src/turtle_neutralize.cpp b/software_training/src/turtle_neutralize.cpp deleted file mode 100644 index cde6687..0000000 --- a/software_training/src/turtle_neutralize.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include - -using namespace std::chrono_literals; - -namespace composition { - -turtle_service_request_node::turtle_service_request_node( - const rclcpp::NodeOptions &options) - : Node("turtle_service_request_node", options) { - // create client - client = this->create_client("/kill"); - - // create callback - timer = this->create_wall_timer( - 2s, std::bind(&turtle_service_request_node::kill, this)); -} - -void turtle_service_request_node::kill() { - - // check if service exists - if (!client->wait_for_service(2s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), - "Interrupted while waiting for service. Exiting!"); - return; - } - RCLCPP_INFO(this->get_logger(), "Service not available after waiting"); - return; - } - - for (std::string &name : turtle_names) { - auto request = std::make_shared(); - request->name = name; - - // create callback to handle response because no 'spin()' is available - - auto callback = - [this](rclcpp::Client::SharedFuture response) - -> void { - (void)response; - RCLCPP_INFO(this->get_logger(), "Turtle Killed"); - rclcpp::shutdown(); // need this or else will keep on executing callback - - // only want to execute once! - }; - - auto result = client->async_send_request(request, callback); - } -} - -} // namespace composition - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(composition::turtle_service_request_node) diff --git a/software_training/src/turtle_publisher.cpp b/software_training/src/turtle_publisher.cpp deleted file mode 100644 index cc352b5..0000000 --- a/software_training/src/turtle_publisher.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include -#include - -using namespace std::chrono_literals; - -namespace composition { - -turtle_publisher::turtle_publisher(const rclcpp::NodeOptions &options) - : Node("turtle_publisher", options) { - - // callback for stationary turtle position - auto stationary_turt_callback = - [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { - this->x_stationary_turt = msg->x; - this->y_stationary_turt = msg->y; - }; - - // callback for moving turtle position - auto moving_turt_callback = - [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { - this->x_moving_turt = msg->x; - this->y_moving_turt = msg->y; - }; - - // publisher callback - auto publisher_callback = [this](void) -> void { - // compute absolute difference in coordinates - double position_x{abs(this->x_stationary_turt - this->x_moving_turt)}; - double position_y{abs(this->y_stationary_turt - this->y_moving_turt)}; - - // create message to publish - auto msg = std::make_unique(); - msg->x_pos = position_x; - msg->y_pos = position_y; - // compute distance using trig - msg->distance = sqrt((position_x * position_x) + (position_y * position_y)); - - // publish message - this->publisher->publish( - std::move(msg)); // must move message because it will be outscope and be - // freed because it is a smart pointer we do not want - // that - need to keep memory. - }; - - // create callback group - really threads that allow things to be executed on - // that thread - callbacks = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - // create callback thread - so that everything with this callback will be on - // one thread - this will ensure mutual exclusion - auto callback_option = rclcpp::SubscriptionOptions(); - callback_option.callback_group = callbacks; - - // set ros2 topic statics - callback_option.topic_stats_options.state = - rclcpp::TopicStatisticsState::Enable; - - // set custom name for this topic statistic - std::string s_name("/statistics/"); - std::string node_name(this->get_name()); - - // topic name will be '/statistics/turtle_publisher' -- naming conventions - // makes it easier to figure which statistics is for which node/component - std::string stat_name = s_name + node_name; - callback_option.topic_stats_options.publish_topic = stat_name.c_str(); - - // instantiate subscribers - stationary_turt_sub = this->create_subscription( - "/stationary_turtle/pose", QUEUE, stationary_turt_callback, - callback_option); - - moving_turt_sub = this->create_subscription( - "/moving_turtle/pose", QUEUE, moving_turt_callback, callback_option); - - // instantiate publisher - publisher = this->create_publisher( - "/difference", QUEUE); - - // set timer for publisher - timer = this->create_wall_timer(3s, publisher_callback, callbacks); -} - -} // namespace composition - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(composition::turtle_publisher) diff --git a/software_training/srv/Software.srv b/software_training/srv/Software.srv deleted file mode 100644 index d857c6a..0000000 --- a/software_training/srv/Software.srv +++ /dev/null @@ -1,3 +0,0 @@ - ---- -bool success diff --git a/src/circular_turtle_node.cpp b/src/circular_turtle_node.cpp new file mode 100644 index 0000000..de6375b --- /dev/null +++ b/src/circular_turtle_node.cpp @@ -0,0 +1,43 @@ +#include "../include/software_training_assignment/circular_turtle_node.hpp" +#include +#include + +using namespace composition; + +circular_turtle::circular_turtle(const rclcpp::NodeOptions &options) : rclcpp::Node("circular_turtle_node"){ + //if this gets called then + + //create a service + timer_ = create_wall_timer(std::chrono::duration(1), std::bind(&circular_turtle::rotate, this)); + +} + +void circular_turtle::rotate() { + + //moving a turtle needs a pub/sub relationship + //need to create a publisher that publishes the velocity message to the turtle sim subscriber + + + + + auto publish_callback = [this](void) -> void{ + auto message = std::make_unique(); + message->linear.x = circular_turtle::coordinates::linear::x; + message->linear.y = circular_turtle::coordinates::linear::y; + message->linear.z = circular_turtle::coordinates::linear::z; + + message->angular.x = circular_turtle::coordinates::angular::x; + message->angular.y = circular_turtle::coordinates::angular::y; + message->angular.z = circular_turtle::coordinates::angular::z; + + this->publisher->publish(std::move(message)); + }; + + + this -> publisher = this -> create_publisher("moving_turtle/cmd_vel", rclcpp::QoS(QUEUE)); //what do I put here lol + timer_ = create_wall_timer(std::chrono::duration(1), publish_callback); + + + + +} \ No newline at end of file diff --git a/src/clear_turtles_node.cpp b/src/clear_turtles_node.cpp new file mode 100644 index 0000000..cf713b9 --- /dev/null +++ b/src/clear_turtles_node.cpp @@ -0,0 +1,31 @@ +#include "../include/software_training_assignment/clear_turtles_node.hpp" + +using namespace composition; + +clear_turtles_node::clear_turtles_node(const rclcpp::NodeOptions &options) : Node("clear_turtles_node") { + + client = this -> create_client("kill"); + + timer_ = create_wall_timer(std::chrono::duration(1), std::bind(&clear_turtles_node::kill, this)); + +} + +void clear_turtles_node::kill() { + //kill all the nodes by sending a request to each name + for(auto turtle : nodes) { + //send a kill request to turtlesim for each turtle name + //first check if the turtle exists + //then delete the turtle + //make requests I guess + auto request = std::make_shared(); + //need a callback function + + auto callback = [this](rclcpp::Client::SharedFuture response) -> void { + std::cout << "finished" << std::endl; + rclcpp::shutdown(); + }; + + client -> async_send_request(request, callback); + } + +} \ No newline at end of file diff --git a/src/moving_turtle_action_node.cpp b/src/moving_turtle_action_node.cpp new file mode 100644 index 0000000..0a5d3e2 --- /dev/null +++ b/src/moving_turtle_action_node.cpp @@ -0,0 +1,132 @@ +#include "../include/software_training_assignment/moving_turtle_action_node.hpp" + + +using namespace composition; +moving_turtle_action_node::moving_turtle_action_node() : Node("moving_turtle_action_node") { + this->publisher = this->create_publisher( + "/moving_turtle/cmd_vel", rclcpp::QoS(QUEUE)); + + auto subscriber_callback = + [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this->moving_turtle_action_server::x = msg->x; + this->moving_turtle_action_server::y = msg->y; + this->moving_turtle_action_server::theta = msg->theta; + this->moving_turtle_action_server::linear_velocity = msg->linear_velocity; + this->moving_turtle_action_server::angular_velocity = msg->angular_velocity; + }; + + + // create subscriber + this->subscriber = this->create_subscription( + "/moving_turtle/pose", QUEUE, subscriber_callback); + + // create action server + this->action_server = + rclcpp_action::create_server( + this, "moving_turtle_action_server", + std::bind(&moving_turtle_action_server::handle_goal, this, _1, _2), + std::bind(&moving_turtle_action_server::handle_cancel, this, _1), + std::bind(&moving_turtle_action_server::handle_accepted, this, _1) + + ); +} + + +rclcpp_action::GoalResponse moving_turtle_action_node::handle_goal( +const rclcpp_action::GoalUUID & uuid, +std::shared_ptr goal) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse moving_turtle_action_node::handle_cancel( + const std::shared_ptr goal_handle +) { + return rclcpp_action::CancelResponse::ACCEPT; +} + +void handle_accepted(const std::shared_ptr goal_handle) { + std::thread{std::bind(&moving_turtle_action_server::execute, this, _1), + goal_handle} + .detach(); +} + +void moving_turtle_action_node::execute(const std::shared_ptr goal_handle) { + const auto goal = goal_handle->get_goal(); + auto start_time = this -> now(); + + float lin_x{0}; + float lin_y{0}; + float lin_z{0}; + + + float ang_x{0}; + float ang_y{0}; + float ang_z{0}; + + while(rclcpp::ok() && (lin_x < goal->linear_pos.x || lin_y < goal->linear_pos.y || + lin_z < goal->linear_pos.z || ang_x < goal->angular_pos.x || + ang_y < goal->angular_pos.y || ang_z < goal->angular_pos.z)) { + + if (goal_handle->is_canceling()) { + + rclcpp::Time curr_time = this->now(); + rclcpp::Duration time = curr_time - start_time; + long int duration{time.nanoseconds()}; + result->duration = duration; + + goal_handle->canceled(std::move(result)); + return; + + + } + auto message = std::make_unique(); + message -> linear.x = (lin_x < goal -> linear_pos.x) ? ++lin_x : lin_x; + message -> lienar.y = (lin_y < goal -> linear_pos.y) ? ++lin_y : lin_y; + message -> linear.z = (lin_z < goal -> linear_pos.z) ? ++lin_z : liny_z; + message -> angular.x = (ang_x < goal -> angular_pos.x) ? ++ang_x : ang_x; + message -> angular.y = (ang_y < goal -> angular_pos.y) ? ++ang_y : ang_y; + message -> angular.z = (ang_z < goal -> angular_pos.z) ? ++ang_z : ang_z; + + + this->publisher->publish(std::move(message)); + + auto feedback = std::make_unique(); + + feedback -> x = this->moving_turtle_action_server::x - lin_x; + feedback -> y = this->moving_turtle_action_server::y - lin_y; + + float theta{0}; + + // scope this stuff cause we dont need it afterwards + { + + float x1{lin_x}, x2{lin_y}, x3{lin_z}; + + float magnitude{static_cast(sqrt((x1 * x1) + (x2 * x2) + (x3 * x3)))}; + + theta = acos(x3 / magnitude); + } + + feedback -> theta = this -> moving_turtle_action_server::theta - theta; + + goal_handle->publish_feedback(std::move(feedback)); + + cycle_rate.sleep(1); + } + + + if (rclcpp::ok()) { + result->sequence = sequence; + + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + rclcpp::Time end = this->now(); + rclcpp::Duration duration = end - start_time; + long int res_time{duration.nseconds()}; + + // fill in result + result->duration = res_time; + goal_handle->succeed(std::move(result)); + + } +} + diff --git a/src/reset_turtle_node.cpp b/src/reset_turtle_node.cpp new file mode 100644 index 0000000..6cdcb02 --- /dev/null +++ b/src/reset_turtle_node.cpp @@ -0,0 +1,28 @@ +#include "../include/software_training_assignment/reset_turtle_node.hpp" + +using namespace composition; + +reset_turtle_node::reset_turtle_node(const rclcpp::NodeOptions &options) : Node("reset_turtle_node"){ + client_ = this -> create_client("/moving_turtle/teleport_absolute"); + service_ = this -> create_service("/reset_moving_turtle", &reset_moving_turtle); //pass it a reference to a funciton +} +void reset_turtle_node::reset_moving_turtle() { + //call turtlesim teleport absolute + timer_ = create_wall_timer(std::chrono::duration(1), std::bind(&reset_turtle_node::reset, this)); +} + +void reset_turtle_node::reset() { + + auto request = std::make_shared(); // how do i interact with turtleX lol the documentation doesn't take a name as input + request -> x = 25; + request -> y = 25; + request -> theta = 0; + + auto callback = [this](rclcpp::Client::SharedFuture response) -> { + + RCLCPP_INFO(this->get_logger(), "Turtle Moved"); + }; + auto result = client_ -> async_send_request(request, callback); + response -> success = true; + +} diff --git a/src/spawn_turtles_node.cpp b/src/spawn_turtles_node.cpp new file mode 100644 index 0000000..84f774f --- /dev/null +++ b/src/spawn_turtles_node.cpp @@ -0,0 +1,33 @@ +#include "../include/software_training_assignment/spawn_turtles_node.hpp" + + +spawn_turtles::spawn_turtles(const rclcpp::NodeOptions &options) : Node("spawn_turtles_node") { + timer_ = create_wall_timer(std::chrono::duration(1), std::bind(&spawn_turtles::spawn, this)); +} + +void spawn_turtles::spawn() { + //make stationary turtle + client = this -> create_client ("spawn"); + auto request1 = std::make_shared(); + request1 -> x = 25; + request1 -> y = 25; + request1 -> theta = 0; + request1-> name = "moving_turtle"; + auto request2 = std::make_shared(); + request2->x = 25; + request2->y = 25; + request2->theta = 0; + request2->name = "stationary_turtle"; + + auto callbackFunction1 = [this](rclcpp::Client::SharedFuture response) -> void{ + std::cout << "sent moving_turtle" + }; + auto callbackFunction2 = [this](rclcpp::Client::SharedFuture response) -> void{ + std::cout << "send stationary turtle" << std::endl; + }; + client -> async_send_request(request1, callbackFunction1); + client -> async_send_request(request2, callbackFunction2); + + + +} \ No newline at end of file diff --git a/src/turtle_distance_node.cpp b/src/turtle_distance_node.cpp new file mode 100644 index 0000000..7db8806 --- /dev/null +++ b/src/turtle_distance_node.cpp @@ -0,0 +1,35 @@ +#include "../include/software_training_assignment/turtle_distance_node.hpp" + +using namespace composition; +turtle_distance_node::turtle_distance_node(const rclcpp::NodeOptions &options) : Node("turtle_distance_node") { + //need to subscribe to pose for turtle + auto callback1 = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this -> turtle1_x = msg -> x; + this -> turtle1_y = msg -> y; + }; + auto callback2 = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this -> turtle2_x = msg -> x; + this -> turtle2_y = msg -> x; + }; + auto subscription_1 = this -> create_subscription("moving_turtle/pose", 10, callback1); + auto subscription_2 = this -> create_subscription("stationary_turtle/pose", 10, callback2); + + auto publisher_callback = [this]() -> void { + float x{abs(this ->turtle1_x - this -> turtle2_x)}; + float y{abs(this ->turtle1_y - this -> turtle2_y)}; + float distance{turtleDistance()}; + auto msg = std::make_unique(); + msg -> x = x; + msg -> y = y; + msg -> distance = distance; + this -> publisher -> publish(std::move(msg)); + }; + + publisher = this -> create_publisher("/difference", 10); + timer = this -> create_wall_timer(std::chrono::duration(1), publisher_callback); +} + +float turtle_distance_node::turtleDistance() { + float distance = sqrtf(pow((turtle1_x - turtle2_x), 2) + pow((turtle1_y - turtle2_y), 2)); + return distance; +} diff --git a/srv/Software.srv b/srv/Software.srv new file mode 100644 index 0000000..e2c59ee --- /dev/null +++ b/srv/Software.srv @@ -0,0 +1,3 @@ + +--- +bool success \ No newline at end of file