diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f23e785 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,188 @@ +cmake_minimum_required(VERSION 3.5) +project(ros_tut) + +# 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_directories(include) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Num.msg" + "srv/AddThreeInts.srv" + "srv/Success.srv" + "msg/Distance.msg" + "action/Software.action" + DEPENDENCIES std_msgs geometry_msgs builtin_interfaces + ) + +# add_executable(talker src/my_publisher.cpp) +# ament_target_dependencies(talker rclcpp std_msgs) +# add_executable(my_node src/my_node.cpp) +# add_executable(listener src/my_subscriber.cpp) +# ament_target_dependencies(listener rclcpp std_msgs) + + +# add_executable(server src/add_two_ints_server.cpp) +# ament_target_dependencies(server +# rclcpp example_interfaces) +# add_executable(client src/add_two_ints_client.cpp) +# ament_target_dependencies(client +# rclcpp example_interfaces) + +# add_executable(clear src/clear_turtles.cpp) +# ament_target_dependencies(clear +# rclcpp ) + +# target_include_directories(my_node PUBLIC +# $ +# $) + +#add kill_turtles as a plugin +add_library(kill_turtles SHARED + src/clear_turtles.cpp) +target_compile_definitions(kill_turtles + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(kill_turtles + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_msgs") +rclcpp_components_register_nodes(kill_turtles "composition::kill_turtles") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(node_plugins "${node_plugins}compositon::kill_turtles;$\n") + +add_library(circle SHARED + src/circle.cpp) +target_compile_definitions(circle + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(circle + "rclcpp" + "rclcpp_components" + "turtlesim" + "geometry_msgs") +rclcpp_components_register_nodes(circle "composition::circle") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(node_plugins "${node_plugins}compositon::circle;$\n") + +add_library(spawn SHARED + src/spawn.cpp) +target_compile_definitions(spawn + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(spawn + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_msgs") +rclcpp_components_register_nodes(spawn "composition::spawn") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(node_plugins "${node_plugins}compositon::spawn;$\n") + +add_library(reset SHARED + src/reset.cpp) +target_compile_definitions(reset + PRIVATE "COMPOSITION_BUILDING_DLL" + ) +ament_target_dependencies(reset + "rclcpp" + "rclcpp_components" + "turtlesim" + "turtlesim" + "std_msgs") +rosidl_target_interfaces(reset ${PROJECT_NAME} "rosidl_typesupport_cpp") # need for custom srv +rclcpp_components_register_nodes(reset "composition::reset") +set(node_plugins "${node_plugins}composition::reset;$) + +add_library(publisher SHARED + src/publisher.cpp) +target_compile_definitions(publisher + PRIVATE "COMPOSITION_BUILDING_DLL" + ) +ament_target_dependencies(publisher + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_msgs") +rosidl_target_interfaces(publisher ${PROJECT_NAME} "rosidl_typesupport_cpp") # need this for custom messages - idk why tho +rclcpp_components_register_nodes(publisher "composition::publisher") +set(node_plugins "${node_plugins}composition::publisher;$") + +# add_library(action SHARED +# src/action.cpp) +# target_compile_definitions(action +# PRIVATE "COMPOSITION_BUILDING_DLL") +# ament_target_dependencies(action +# "rclcpp" +# "rclcpp_components" +# "turtlesim" +# "rclcpp_action" +# "std_msgs" +# "geometry_msgs") +# rosidl_target_interfaces(action ${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(action PLUGIN "composition::action" EXECUTABLE action) +add_library(turtle_action_server SHARED + src/action.cpp) +target_compile_definitions(turtle_action_server + PRIVATE "COMPOSITION_BUILDING_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::action" EXECUTABLE action) +# rclcpp_components_register_nodes(turtle_action_server "composition::action") +rclcpp_components_register_nodes(turtle_action_server "composition::action") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(turtle_action_server "${node_plugins}compositon::action;$\n") + + + +install(TARGETS + # my_node + # talker + # listener + kill_turtles + circle + spawn + reset + turtle_action_server + publisher + # server + # client + RUNTIME DESTINATION lib + LIBRARY DESTINATION lib + ARCHIVE DESTINATION bin) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/action/Software.action b/action/Software.action new file mode 100644 index 0000000..545f94b --- /dev/null +++ b/action/Software.action @@ -0,0 +1,8 @@ +geometry_msgs/Vector3 linear_pos +geometry_msgs/Vector3 angular_pos +--- +uint64 duration +--- +float32 x_pos +float32 y_pos +float32 theta_pos diff --git a/include/ros-tut/action.hpp b/include/ros-tut/action.hpp new file mode 100644 index 0000000..20d145c --- /dev/null +++ b/include/ros-tut/action.hpp @@ -0,0 +1,57 @@ +#ifndef ACTION_HPP_ +#define ACTION_HPP_ + +// #include + +#include +#include +#include // cmd_vel publisher message +#include +#include +#include // ros2 time header +#include // ros2 action header +#include +#include +#include // header for message to get moving turt position + +namespace composition { + +class action : public rclcpp::Node { + public: + explicit action(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; + + rclcpp::Publisher::SharedPtr publisher; + + rclcpp::Subscription::SharedPtr subscriber; + + 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); + + // handle accepetd callback function + 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}; +}; + +} // namespace composition + +#endif // MOVING_TURTLE_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/include/ros-tut/circle.hpp b/include/ros-tut/circle.hpp new file mode 100644 index 0000000..d61d9f9 --- /dev/null +++ b/include/ros-tut/circle.hpp @@ -0,0 +1,35 @@ +// #ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +// #define TURTLE_SERVICE_REQUEST_NODE_HPP_ +#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +#define TURTLE_SERVICE_REQUEST_NODE_HPP_ + + + +#include +#include +#include +#include +#include + +#include + +// #include +#include +#include "geometry_msgs/msg/twist.hpp" +namespace composition{ + +class circle : public rclcpp::Node { + +public: + explicit circle(const rclcpp::NodeOptions &options); + +private: + rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; + + void circleBoi(); + +}; +} + +#endif diff --git a/include/ros-tut/publisher.hpp b/include/ros-tut/publisher.hpp new file mode 100644 index 0000000..4a92b91 --- /dev/null +++ b/include/ros-tut/publisher.hpp @@ -0,0 +1,34 @@ +// #include + +#include +#include +#include +#include + +namespace composition { + +class publisher : public rclcpp::Node { + public: + explicit publisher(const rclcpp::NodeOptions &options); + + private: + rclcpp::Publisher::SharedPtr publisher_val; + + rclcpp::Subscription::SharedPtr sitting_turtle; + rclcpp::Subscription::SharedPtr moving_turtle; + + rclcpp::TimerBase::SharedPtr timer; + + rclcpp::CallbackGroup::SharedPtr callbacks; + void publish(); + float x_sitting_turtle; + float y_sitting_turtle; + + float x_moving_turtle; + float y_moving_turtle; + + float distance; + + static const unsigned int QUEUE{10}; +}; +} // namespace composition \ No newline at end of file diff --git a/include/ros-tut/reset.hpp b/include/ros-tut/reset.hpp new file mode 100644 index 0000000..57fdddd --- /dev/null +++ b/include/ros-tut/reset.hpp @@ -0,0 +1,37 @@ +#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +#define TURTLE_SERVICE_REQUEST_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std::placeholders; +using namespace std::chrono_literals; + +// #include "rclcpp/rclcpp.hpp" +// #include "std_msgs/msg/string.hpp" +// #include "ros_tut/srv/add_three_ints.hpp" + +namespace composition { + +class reset : public rclcpp::Node { + public: + // SOFTWARE_TRAINING_PUBLIC //what is this? + explicit reset(const rclcpp::NodeOptions &options); + + private: + rclcpp::Service::SharedPtr service; + rclcpp::TimerBase::SharedPtr timer; + + // all the turtles + rclcpp::Client::SharedPtr client; + // SOFTWARE_TRAINING_LOCAL //what is this? + void reset_service(const std::shared_ptr request, + std::shared_ptr response); +}; +} // namespace composition +#endif \ No newline at end of file diff --git a/include/ros-tut/spawn.hpp b/include/ros-tut/spawn.hpp new file mode 100644 index 0000000..8e52e1a --- /dev/null +++ b/include/ros-tut/spawn.hpp @@ -0,0 +1,43 @@ +#ifndef SPAWN_TURTLE_NODELET_HPP_ +#define SPAWN_TURTLE_NODELET_HPP_ + +// #include +// #include +#include +// #include +#include +#include +#include + +namespace composition { + +class spawn : public rclcpp::Node { + +public: + explicit spawn(const rclcpp::NodeOptions &options); + +private: + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + int NUMBER_OF_TURTLES = 2; + + typedef struct turtle_info { + std::string name; + float x; + float y; + } turtle_info; + + std::vector turtle_descriptions = { + {"stationary_turtle", 5, 5}, + {"moving_turtle", 25, 10} + }; + + std::map turtle_description; + + void spawn_offspring(); + +}; + +} +#endif \ No newline at end of file diff --git a/include/visibility.hpp b/include/visibility.hpp new file mode 100644 index 0000000..907a909 --- /dev/null +++ b/include/visibility.hpp @@ -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/msg/Distance.msg b/msg/Distance.msg new file mode 100644 index 0000000..9e3c23b --- /dev/null +++ b/msg/Distance.msg @@ -0,0 +1,3 @@ +float64 x_pos +float64 y_pos +float64 distance \ No newline at end of file diff --git a/msg/Num.msg b/msg/Num.msg new file mode 100644 index 0000000..5a7c026 --- /dev/null +++ b/msg/Num.msg @@ -0,0 +1 @@ +int64 num \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f910183 --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + + ros_tut + 0.0.0 + TODO: Package description + ari + TODO: License declaration + + ament_cmake + rclcpp + + ament_lint_auto + ament_lint_common + rosidl_default_generators + rclcpp_components + std_msgs + turtlesim + rcutils + geometry_msgs + builtin_interfaces + rosidl_default_generators + rclcpp_action + + + rclcpp_components + launch_ros + + rcutils + turtlesim + geometry_msgs + rosidl_default_runtime + rclcpp_action + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/action.cpp b/src/action.cpp new file mode 100644 index 0000000..7612fec --- /dev/null +++ b/src/action.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + +action::action(const rclcpp::NodeOptions &options) : Node("action", options) { + // create publisher + this->publisher = this->create_publisher("/moving_turtle/cmd_vel", rclcpp::QoS(QUEUE)); + + auto subscriber_callback = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this->action::x = msg->x; + this->action::y = msg->y; + this->action::theta = msg->theta; + this->action::linear_velocity = msg->linear_velocity; + this->action::angular_velocity = msg->angular_velocity; + }; + + this->subscriber = this->create_subscription("/moving_turtle/pose", QUEUE, subscriber_callback); + + this->action_server = rclcpp_action::create_server( + this, "action", std::bind(&action::handle_goal, this, _1, _2), std::bind(&action::handle_cancel, this, _1), + std::bind(&action::handle_accepted, this, _1) + + ); +} + +rclcpp_action::GoalResponse action::handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + (void)uuid; + RCLCPP_INFO(this->get_logger(), "GOAL RECIIEIIVEIIVEID"); + 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 action::handle_cancel(const std::shared_ptr goal_handle) { + (void)goal_handle; // not needed + + RCLCPP_INFO(this->get_logger(), "Cancelling goal :("); + + return rclcpp_action::CancelResponse::ACCEPT; +} + +void action::handle_accepted(const std::shared_ptr goal_handle) { + std::thread{std::bind(&action::execute, this, _1), goal_handle}.detach(); +} + +void action::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 + + const auto goal = goal_handle->get_goal(); + + std::unique_ptr feedback = + std::make_unique(); + + std::unique_ptr result = std::make_unique(); + + float &curr_x = feedback->x_pos; + float &curr_y = feedback->y_pos; + float &curr_theta = feedback->theta_pos; + + 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_INFO(this->get_logger(), "Goal Canceled"); + + 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_cmd_vel = std::make_unique(); + + 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; + + this->publisher->publish(std::move(message_cmd_vel)); + + // now compute feedback + curr_x = this->action::x - lin_x; + curr_y = this->action::y - lin_y; + float theta{0}; + 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->action::theta - theta; + + goal_handle->publish_feedback(std::move(feedback)); + + cycle_rate.sleep(); + } + + // if goal is done + if (rclcpp::ok()) { + rclcpp::Time end = this->now(); + rclcpp::Duration duration = end - start_time; + long int res_time{duration.nanoseconds()}; + result->duration = res_time; + // goal_handle->succeed(std::move(result)); + RCLCPP_INFO(this->get_logger(), "Finish Executing Goal"); + } +} + +} // namespace composition + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::action) diff --git a/src/add_two_ints_client.cpp b/src/add_two_ints_client.cpp new file mode 100644 index 0000000..0f66ede --- /dev/null +++ b/src/add_two_ints_client.cpp @@ -0,0 +1,47 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if (argc != 3) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y"); + return 1; + } + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_client"); + rclcpp::Client::SharedPtr client = + node->create_client("add_two_ints"); + + auto request = std::make_shared(); + request->a = atoll(argv[1]); + request->b = atoll(argv[2]); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints"); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/src/add_two_ints_server.cpp b/src/add_two_ints_server.cpp new file mode 100644 index 0000000..9371546 --- /dev/null +++ b/src/add_two_ints_server.cpp @@ -0,0 +1,28 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include + +void add(const std::shared_ptr request, + std::shared_ptr response) +{ + response->sum = request->a + request->b; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld", + request->a, request->b); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + rclcpp::Service::SharedPtr service = + node->create_service("add_two_ints", &add); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/src/circle.cpp b/src/circle.cpp new file mode 100644 index 0000000..9662f60 --- /dev/null +++ b/src/circle.cpp @@ -0,0 +1,23 @@ +#include +using namespace std::chrono_literals; + + +namespace composition { + + circle::circle(const rclcpp::NodeOptions &options) : Node("circle", options) { + publisher = create_publisher("/turtle1/cmd_vel", 10); + timer = create_wall_timer(2s, std::bind(&circle::circleBoi, this)); + } + + void circle::circleBoi(){ + auto message = geometry_msgs::msg::Twist(); + message.linear.x = 1; + message.angular.z = 1; + publisher->publish(message); + } + +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(composition::circle) + diff --git a/src/clear_turtles.cpp b/src/clear_turtles.cpp new file mode 100644 index 0000000..2ac15eb --- /dev/null +++ b/src/clear_turtles.cpp @@ -0,0 +1,75 @@ +#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +#define TURTLE_SERVICE_REQUEST_NODE_HPP_ + +#include +#include +#include +#include + +#include + +#include + + +using namespace std::placeholders; +using namespace std::chrono_literals; + +// #include "rclcpp/rclcpp.hpp" +// #include "std_msgs/msg/string.hpp" +// #include "ros_tut/srv/add_three_ints.hpp" + +namespace composition { + + +class kill_turtles : public rclcpp::Node { + public: + // SOFTWARE_TRAINING_PUBLIC //what is this? + explicit kill_turtles(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 //what is this? + void clear(); +}; + +#endif + + +kill_turtles::kill_turtles(const rclcpp::NodeOptions &options) : Node("kill_turtles", options) { + client = create_client("/kill"); + + timer = create_wall_timer(2s, std::bind(&kill_turtles::clear, this)); // don't understand line +} + +void kill_turtles::clear() { + if (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the 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; + + // using ServiceResponseFuture = kill_turtles::SharedFuture; + auto response_received_callback = [this](rclcpp::Client::SharedFuture response) { + RCLCPP_INFO(this->get_logger(), "POGGERS"); + (void)response; + }; + + auto future_result = client->async_send_request(request, response_received_callback); + } +} +} +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::kill_turtles) //what is this?? diff --git a/src/my_node.cpp b/src/my_node.cpp new file mode 100644 index 0000000..369c19e --- /dev/null +++ b/src/my_node.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world ros-tut package\n"); + return 0; +} diff --git a/src/my_publisher.cpp b/src/my_publisher.cpp new file mode 100644 index 0000000..37fbf93 --- /dev/null +++ b/src/my_publisher.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a +* member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ + public: + MinimalPublisher() + : Node("minimal_publisher"), count_(0) + { + publisher_ = this->create_publisher("topic", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&MinimalPublisher::timer_callback, this)); + } + + private: + void timer_callback() + { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/my_subscriber.cpp b/src/my_subscriber.cpp new file mode 100644 index 0000000..733fcac --- /dev/null +++ b/src/my_subscriber.cpp @@ -0,0 +1,31 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +using std::placeholders::_1; + +class MinimalSubscriber : public rclcpp::Node +{ + public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + subscription_ = this->create_subscription( + "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + } + + private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg) const + { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + } + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/publisher.cpp b/src/publisher.cpp new file mode 100644 index 0000000..8309b94 --- /dev/null +++ b/src/publisher.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +// #include + +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + +publisher::publisher(const rclcpp::NodeOptions &options) : Node("publisher", options) { + publisher_val = create_publisher("/turtle_distance", 10); + timer = create_wall_timer(1000ms, std::bind(&publisher::publish, this)); + + auto sitting_turtle = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + x_sitting_turtle = msg->x; + y_sitting_turtle = msg->x; + }; + + auto moving_turtle = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + x_moving_turtle = msg->x; + y_moving_turtle = msg->y; + }; + + auto callback_option = rclcpp::SubscriptionOptions(); + callback_option.callback_group = callbacks; + + // set ros2 topic statics + callback_option.topic_stats_options.state = + rclcpp::TopicStatisticsState::Enable; + + + this->sitting_turtle = this->create_subscription( + "/stationary_turtle/pose", QUEUE, sitting_turtle, callback_option); + + this->moving_turtle = this->create_subscription( + "/moving_turtle/pose", QUEUE, moving_turtle, callback_option ); +} + +void publisher::publish() { + double pos_y{abs(y_sitting_turtle - y_moving_turtle)}; + double pos_x{abs(x_sitting_turtle - x_sitting_turtle)}; + auto msg = std::make_unique(); + msg->x_pos = pos_x; + msg->y_pos = pos_y; + msg->distance = sqrt((pos_x * pos_x) + (pos_y * pos_y)); + + publisher_val->publish(std::move(msg)); +} +} + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::publisher) \ No newline at end of file diff --git a/src/reset.cpp b/src/reset.cpp new file mode 100644 index 0000000..57d814e --- /dev/null +++ b/src/reset.cpp @@ -0,0 +1,71 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std::placeholders; +using namespace std::chrono_literals; + +namespace composition { + +reset::reset(const rclcpp::NodeOptions &options) : Node("reset", options) { + + client = create_client("/moving_turtle/teleport_absolute"); + + // create service + service = this->create_service( + "/reset", std::bind(&reset::reset_service, this, std::placeholders::_1, std::placeholders::_2)); // what does this do???? + + +} + +void reset::reset_service(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->result = false; + return; + } + RCLCPP_INFO(this->get_logger(), "Service is not available! Exit!"); + response->result = false; + return; + } + + auto client_request = std::make_shared(); + + // fill request data + client_request->x = 5.4; + client_request->y = 5.4; + client_request->theta = 0; + + // create response callback + auto response_callback = [this](rclcpp::Client::SharedFuture future) -> void { + (void)future; // not needed + RCLCPP_INFO(this->get_logger(), "Turtle Moved!!! YAYAYYAYAYAY"); + }; + + // send client request + auto result = client->async_send_request(client_request, response_callback); + + RCLCPP_INFO(this->get_logger(), "I LIKE TURTLES!!!!!!"); + + response->result = true; +} + +} // namespace composition + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::reset) diff --git a/src/spawn.cpp b/src/spawn.cpp new file mode 100644 index 0000000..e3db920 --- /dev/null +++ b/src/spawn.cpp @@ -0,0 +1,56 @@ +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace composition { + spawn::spawn(const rclcpp::NodeOptions &options) : Node("spawn", options) { + client = this->create_client("/spawn"); + timer = create_wall_timer(2s, std::bind(&spawn::spawn_offspring, this)); + } + + void spawn::spawn_offspring(){ + if (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available after waiting"); + return; + } + + for (const turtle_info &turt_info : turtle_descriptions) { //whats the point in the & and const here? + + // create request + std::unique_ptr request = + std::make_unique(); + + // fill in repsonse + request->name = turt_info.name; + request->x = turt_info.x; + request->y = turt_info.y; + + // 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); + } + + } + +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(composition::spawn) + diff --git a/srv/AddThreeInts.srv b/srv/AddThreeInts.srv new file mode 100644 index 0000000..b2e646f --- /dev/null +++ b/srv/AddThreeInts.srv @@ -0,0 +1,5 @@ +int64 a +int64 b +int64 c +--- +int64 sum diff --git a/srv/AddTwoInts.srv b/srv/AddTwoInts.srv new file mode 100644 index 0000000..3bef723 --- /dev/null +++ b/srv/AddTwoInts.srv @@ -0,0 +1,4 @@ +int64 a +int64 b +--- +int64 sum \ No newline at end of file diff --git a/srv/Success.srv b/srv/Success.srv new file mode 100644 index 0000000..2969a95 --- /dev/null +++ b/srv/Success.srv @@ -0,0 +1,2 @@ +--- +bool result \ No newline at end of file