diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fb069af --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build/ +install/ +log/ +venv/ +__pycache__/ diff --git a/graph_pwm.py b/graph_pwm.py new file mode 100644 index 0000000..7fcd90e --- /dev/null +++ b/graph_pwm.py @@ -0,0 +1,27 @@ +import matplotlib.pyplot as plt +import numpy as np +from scipy import signal + +clock_speed = 84000000 +prescaler = 839 +period = 1999 +pulse = 1000 + +# okay so pwm_freq = clock_speed / prescaler * period +# duty cycle = pulse / period + +timer_tick_freq = clock_speed / (prescaler + 1) +pwm_freq = timer_tick_freq / (period + 1) +duty_cycle = (pulse / (period + 1)) * 100 + +time = np.linspace(0, 0.06, 1000) +wave = signal.square(2 * np.pi * pwm_freq * time, duty=duty_cycle/100) + +plt.figure(figsize=(10, 4)) +plt.plot(time * 1000, wave, color='green', linewidth=2) +plt.title(f'PWM Output ({pwm_freq}Hz, {duty_cycle}% Duty Cycle)') +plt.xlabel('Time (ms)') +plt.ylabel('On/Off') +plt.ylim(-1.5, 1.5) +plt.grid(True) +plt.show() \ No newline at end of file diff --git a/pwm_config.png b/pwm_config.png new file mode 100644 index 0000000..d0ce212 Binary files /dev/null and b/pwm_config.png differ diff --git a/software_training_assignment/uwrt_software_trainining/.devcontainer/devcontainer.json b/software_training_assignment/uwrt_software_trainining/.devcontainer/devcontainer.json new file mode 100644 index 0000000..e9effb7 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/.devcontainer/devcontainer.json @@ -0,0 +1,27 @@ +// For format details, see https://aka.ms/devcontainer.json. For config options, see the +// README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile +{ + "name": "Existing Dockerfile", + "build": { + // Sets the run context to one level up instead of the .devcontainer folder. + "context": "..", + // Update the 'dockerFile' property if you aren't using the standard 'Dockerfile' filename. + "dockerfile": "../Dockerfile.dev" + }, + "containerEnv": { "DISPLAY": "host.docker.internal:0.0" } + + // Features to add to the dev container. More info: https://containers.dev/features. + // "features": {}, + + // Use 'forwardPorts' to make a list of ports inside the container available locally. + // "forwardPorts": [], + + // Uncomment the next line to run commands after the container is created. + // "postCreateCommand": "cat /etc/os-release", + + // Configure tool-specific properties. + // "customizations": {}, + + // Uncomment to connect as an existing user other than the container default. More info: https://aka.ms/dev-containers-non-root. + // "remoteUser": "devcontainer" +} diff --git a/software_training_assignment/uwrt_software_trainining/.gitignore b/software_training_assignment/uwrt_software_trainining/.gitignore new file mode 100644 index 0000000..40f0bf4 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/.gitignore @@ -0,0 +1 @@ +/software_training/launch/__pycache__/ \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/Dockerfile.dev b/software_training_assignment/uwrt_software_trainining/Dockerfile.dev new file mode 100644 index 0000000..d092724 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/Dockerfile.dev @@ -0,0 +1,15 @@ +# Start with the desktop ROS Galactic 20.04 image +FROM osrf/ros:galactic-desktop + +# This dockerfile assumes you'll share your development folder from your computer over to the container, +# so we don't do `rosdep install` for you, or clone the repository. + +# update everything +RUN sudo apt update -y --no-install-recommends && sudo apt dist-upgrade -y +# Ensure rosinstall and rosdep are installed and up to date +RUN sudo apt install -y python3-rosdep + +# build via docker build -f .\Dockerfile.dev -t uwrt_dev_image . +# run via docker run --name uwrt_dev_container -e DISPLAY=host.docker.internal:0.0 -it -v uwrt_dev_image + + diff --git a/software_training_assignment/uwrt_software_trainining/README.md b/software_training_assignment/uwrt_software_trainining/README.md new file mode 100644 index 0000000..eb76ad0 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/README.md @@ -0,0 +1,29 @@ +# uwrt_software_trainining + +This is a solution of the software training with some added bugs that discourages one from copying and pasting the code. This code is meant to +serve as a guide to those starting out the training. It makes use of some ROS2 concepts not covered in the training as it is meant to encourage you to +go out and learn different features of ROS2. + +This code covers core concepts of ROS2 such as +components, topic statistic, callback groups, extending rclcpp::Node, +the creation of pubs/subs, services/clients, action servers, and multi-threaded callbacks. + +The use of the turtlesim package is needed to complete this training. https://docs.ros.org/en/foxy/Tutorials/Turtlesim/Introducing-Turtlesim.html + +**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)** +1. Clears any existing turtles +2. Create a component that moves 'turtle1' in a circular motion +3. Spawns a turtle named "stationary_turtle" at x = 5, y = 5 + Spawns a second turtle named "moving_turtle" at x = 25, y = 10 +4. Create a service that resets the "moving_turtle" to its starting position. The service response should be whether or not it was successful. +5. 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. + +6. 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. + + diff --git a/software_training_assignment/uwrt_software_trainining/software_training/CMakeLists.txt b/software_training_assignment/uwrt_software_trainining/software_training/CMakeLists.txt new file mode 100644 index 0000000..6585b33 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/CMakeLists.txt @@ -0,0 +1,93 @@ +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_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/Software.msg" + "srv/Software.srv" + "action/Software.action" + DEPENDENCIES std_msgs geometry_msgs builtin_interfaces +) +ament_export_dependencies(rosidl_default_runtime) + +set(node_plugins "") + +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") +set(node_plugins "${node_plugins}composition::turtle_service_request_node;\\$\n") + +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;\\$\n") + +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") +rclcpp_components_register_nodes(turtle_pub "composition::turtle_publisher") +set(node_plugins "${node_plugins}composition::turtle_publisher;\\$\n") + +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" "std_msgs") +rosidl_target_interfaces(turtle_service ${PROJECT_NAME} "rosidl_typesupport_cpp") +rclcpp_components_register_nodes(turtle_service "composition::reset_moving_turtle_service") +set(node_plugins "${node_plugins}composition::reset_moving_turtle_service;\\$\n") + +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;\\$\n") + +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") +rclcpp_components_register_node(turtle_action_server PLUGIN "composition::moving_turtle_action_server" EXECUTABLE moving_action_server) + +install(TARGETS + turtle_request + turtle_spawn + turtle_pub + turtle_service + turtle_action_server + cmd_vel_publisher + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_package() \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/action/Software.action b/software_training_assignment/uwrt_software_trainining/software_training/action/Software.action new file mode 100644 index 0000000..ac77e1c --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/action/Software.action @@ -0,0 +1,16 @@ +# Goal Request /moving_turtle/cmd_vel +geometry_msgs/Vector3 linear_pos +geometry_msgs/Vector3 angular_pos + +# Result - time +--- +uint64 duration + +--- +# Feedback /moving_turtle/pose +float32 x_pos +float32 y_pos +float32 theta_pos + + + diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/cmd_vel_moving_turt_publisher.hpp b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/cmd_vel_moving_turt_publisher.hpp new file mode 100644 index 0000000..0130798 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/cmd_vel_moving_turt_publisher.hpp @@ -0,0 +1,43 @@ +#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: + rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; + + static constexpr unsigned int QUEUE{10}; + + struct coordinates { + struct linear { + static constexpr float x = 2.0; + static constexpr float y = 0.0; + static constexpr float z = 0.0; + }; + struct angular { + static constexpr float x = 0.0; + static constexpr float y = 0.0; + static constexpr float z = 2.0; + }; + }; +}; + +} // namespace composition + +#endif // CMD_VEL_MOVING_TURT_PUBLISHER_HPP_ diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/moving_turtle_action_server.hpp b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/moving_turtle_action_server.hpp new file mode 100644 index 0000000..3e27f27 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/moving_turtle_action_server.hpp @@ -0,0 +1,58 @@ +#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); + + using GoalHandleActionServer = + rclcpp_action::ServerGoalHandle; + +private: + rclcpp_action::Server::SharedPtr action_server; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscriber; + + SOFTWARE_TRAINING_LOCAL + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + + SOFTWARE_TRAINING_LOCAL + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + + SOFTWARE_TRAINING_LOCAL + void handle_accepted(const std::shared_ptr goal_handle); + + void execute(const std::shared_ptr goal_handle); + + float x{0.0f}; + float y{0.0f}; + float theta{0.0f}; + float linear_velocity{0.0f}; + float angular_velocity{0.0f}; + + static constexpr unsigned int QUEUE{10}; +}; + +} // namespace composition + +#endif // MOVING_TURTLE_ACTION_SERVER_HPP_ diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/reset_moving_turtle_service.hpp b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/reset_moving_turtle_service.hpp new file mode 100644 index 0000000..645bc56 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/reset_moving_turtle_service.hpp @@ -0,0 +1,44 @@ +#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); + + struct reset_coordinates { + static constexpr float x = 25.0; + static constexpr float y = 10.0; + static constexpr float theta = 0.0; + }; +}; + +} // namespace composition + +#endif // RESET_MOVING_TURTLE_SERVICE_HPP_ diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/spawn_turtle_nodelet.hpp b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/spawn_turtle_nodelet.hpp new file mode 100644 index 0000000..3934be7 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/spawn_turtle_nodelet.hpp @@ -0,0 +1,44 @@ +#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}; + + struct turtle_info { + float x_pos; + float y_pos; + float rad; + }; + + std::vector turtle_names{"stationary_turtle", "moving_turtle"}; + std::vector turtle_bio{{5.0, 5.0, 0.0}, {25.0, 10.0, 0.0}}; + + std::map turtle_description; +}; + +} // namespace composition + +#endif // SPAWN_TURTLE_NODELET_HPP_ \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/turtle_neutralize.hpp b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/turtle_neutralize.hpp new file mode 100644 index 0000000..90061d1 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/turtle_neutralize.hpp @@ -0,0 +1,32 @@ +#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 (not turtle1, the moving turtle) + std::vector turtle_names = {"moving_turtle", "stationary_turtle"}; + + SOFTWARE_TRAINING_LOCAL + void kill(); +}; + +} // namespace composition +#endif // TURTLE_SERVICE_REQUEST_NODE_HPP_ \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/turtle_publisher.hpp b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/turtle_publisher.hpp new file mode 100644 index 0000000..6119afc --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/turtle_publisher.hpp @@ -0,0 +1,37 @@ +#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: + rclcpp::Subscription::SharedPtr stationary_turt_sub; + rclcpp::Subscription::SharedPtr moving_turt_sub; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::CallbackGroup::SharedPtr callbacks; + + float x_stationary_turt{0.0f}; + float y_stationary_turt{0.0f}; + float x_moving_turt{0.0f}; + float y_moving_turt{0.0f}; + + float total_distance{0.0f}; + + static const unsigned int QUEUE{10}; +}; +} // namespace composition + +#endif // TURTLE_PUBLISHER_HPP_ \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/visibility.h b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/visibility.h new file mode 100644 index 0000000..7465546 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/include/software_training/visibility.h @@ -0,0 +1,65 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SOFTWARE_TRAINING__VISIBILITY_H_ +#define SOFTWARE_TRAINING__VISIBILITY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + +#ifdef __GNUC__ +#define SOFTWARE_TRAINING_EXPORT __attribute__((dllexport)) +#define SOFTWARE_TRAINING_IMPORT __attribute__((dllimport)) +#else +#define SOFTWARE_TRAINING_EXPORT __declspec(dllexport) +#define SOFTWARE_TRAINING_IMPORT __declspec(dllimport) +#endif + +#ifdef SOFTWARE_TRAINING_DLL +#define SOFTWARE_TRAINING_PUBLIC SOFTWARE_TRAINING_EXPORT +#else +#define SOFTWARE_TRAINING_PUBLIC SOFTWARE_TRAINING_IMPORT +#endif + +#define SOFTWARE_TRAINING_PUBLIC_TYPE SOFTWARE_TRAINING_PUBLIC + +#define SOFTWARE_TRAINING_LOCAL + +#else + +#define SOFTWARE_TRAINING_EXPORT __attribute__((visibility("default"))) +#define SOFTWARE_TRAINING_IMPORT + +#if __GNUC__ >= 4 +#define SOFTWARE_TRAINING_PUBLIC __attribute__((visibility("default"))) +#define SOFTWARE_TRAINING_LOCAL __attribute__((visibility("hidden"))) +#else +#define SOFTWARE_TRAINING_PUBLIC +#define SOFTWARE_TRAINING_LOCAL +#endif + +#define SOFTWARE_TRAINING_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // SOFTWARE_TRAINING__VISIBILITY_H_ diff --git a/software_training_assignment/uwrt_software_trainining/software_training/launch/software_training.launch.py b/software_training_assignment/uwrt_software_trainining/software_training/launch/software_training.launch.py new file mode 100644 index 0000000..cd79bf7 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/launch/software_training.launch.py @@ -0,0 +1,47 @@ +import launch +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + """Generate launch description with multiple components.""" + turtlesim_node = Node( + package='turtlesim', + executable='turtlesim_node', + name='sim' + ) + + container = ComposableNodeContainer( + name='software_training_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='software_training', + plugin='composition::turtle_service_request_node', + name='turtle_request'), + ComposableNode( + package='software_training', + plugin='composition::spawn_turtle_nodelet', + name='turtle_spawn'), + ComposableNode( + package='software_training', + plugin='composition::cmd_vel_moving_turt_publisher', + name='cmd_vel_publisher'), + ComposableNode( + package='software_training', + plugin='composition::reset_moving_turtle_service', + name='turtle_service'), + ComposableNode( + package='software_training', + plugin='composition::turtle_publisher', + name='turtle_pub'), + ComposableNode( + package='software_training', + plugin='composition::moving_turtle_action_server', + name='turtle_action_server'), + ], + output='screen', + ) + + return launch.LaunchDescription([turtlesim_node, container]) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/msg/Software.msg b/software_training_assignment/uwrt_software_trainining/software_training/msg/Software.msg new file mode 100644 index 0000000..8dd0f54 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/msg/Software.msg @@ -0,0 +1,3 @@ +float64 x_pos +float64 y_pos +float64 distance diff --git a/software_training_assignment/uwrt_software_trainining/software_training/package.xml b/software_training_assignment/uwrt_software_trainining/software_training/package.xml new file mode 100644 index 0000000..cc3cfe2 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/package.xml @@ -0,0 +1,40 @@ + + + + software_training + 0.0.0 + Software Training Package + niiquaye + Apache-2.0 + + ament_cmake + + rclcpp_components + rclcpp + std_msgs + turtlesim + rcutils + geometry_msgs + builtin_interfaces + rosidl_default_generators + rclcpp_action + + rclcpp + rclcpp_components + launch_ros + std_msgs + rcutils + turtlesim + geometry_msgs + rosidl_default_runtime + rclcpp_action + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/src/cmd_vel_moving_turt_publisher.cpp b/software_training_assignment/uwrt_software_trainining/software_training/src/cmd_vel_moving_turt_publisher.cpp new file mode 100644 index 0000000..d2706f2 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/src/cmd_vel_moving_turt_publisher.cpp @@ -0,0 +1,35 @@ +#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 = coordinates::linear::x; + message->linear.y = coordinates::linear::y; + message->linear.z = coordinates::linear::z; + + message->angular.x = coordinates::angular::x; + message->angular.y = coordinates::angular::y; + message->angular.z = coordinates::angular::z; + + this->publisher->publish(std::move(message)); + }; + + // create the publisher + // also create a QoS object with a history/depth of 10 calls + this->publisher = this->create_publisher( + "/turtle1/cmd_vel", rclcpp::QoS(QUEUE)); + + this->timer = this->create_wall_timer(100ms, publisher_callback); +} + +} // namespace composition + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(composition::cmd_vel_moving_turt_publisher) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/src/moving_turtle_action_server.cpp b/software_training_assignment/uwrt_software_trainining/software_training/src/moving_turtle_action_server.cpp new file mode 100644 index 0000000..93b32ec --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/src/moving_turtle_action_server.cpp @@ -0,0 +1,141 @@ +#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)); + + auto subscriber_callback = + [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this->x = msg->x; + this->y = msg->y; + this->theta = msg->theta; + this->linear_velocity = msg->linear_velocity; + this->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; // prevents compiler warnings + RCLCPP_INFO(this->get_logger(), "Goal Received"); + 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(), "Received Request to cancel goal!"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void moving_turtle_action_server::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_server::execute( + const std::shared_ptr goal_handle) { + + + rclcpp::Time start_time = this->now(); + RCLCPP_INFO(this->get_logger(), "Executing Goal"); + rclcpp::Rate cycle_rate{10}; + + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + float &curr_x = feedback->x_pos; + float &curr_y = feedback->y_pos; + float &curr_theta = feedback->theta_pos; + + // track physical distance + + float target_x = goal->linear_pos.x; + float target_y = goal->linear_pos.y; + float distance_to_goal = 100.0f; + + while (rclcpp::ok() && distance_to_goal > 0.1f) { + // here we update euclidian distance + distance_to_goal = std::sqrt(std::pow(target_x - this->x, 2) + std::pow(target_y - this->y, 2)); + + // check if goal has been reached + if (goal_handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal Canceled"); + rclcpp::Duration time = this->now() - start_time; + result->duration = static_cast(time.nanoseconds()); + goal_handle->canceled(std::move(result)); + return; + } + + // create message + auto message_cmd_vel = std::make_unique(); + + float angle_to_target = std::atan2(target_y - this->y, target_x - this->x); + float angle_error = angle_to_target - this->theta; + + // normalize angle to prevent wild spinning + while (angle_error > M_PI) + angle_error -= 2.0f * M_PI; + while (angle_error < -M_PI) + angle_error += 2.0f * M_PI; + + message_cmd_vel->linear.x = 1.0f * distance_to_goal; + message_cmd_vel->angular.z = 4.0f * angle_error; + + // publish message + this->publisher->publish(std::move(message_cmd_vel)); + + // compute feedback + auto feedback_msg = std::make_shared(); + feedback_msg->x_pos = distance_to_goal; + feedback_msg->y_pos = 0.0f; + feedback_msg->theta_pos = angle_error; + + goal_handle->publish_feedback(feedback_msg); + + cycle_rate.sleep(); + } // brutal problem + + // stop when loop stops + auto stop_msg = std::make_unique(); + this->publisher->publish(std::move(stop_msg)); + + if (rclcpp::ok()) { + rclcpp::Duration duration = this->now() - start_time; + result->duration = static_cast(duration.nanoseconds()); + goal_handle->succeed(std::move(result)); + RCLCPP_INFO(this->get_logger(), "Finish Executing Goal"); + } +} + +} // namespace composition + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(composition::moving_turtle_action_server) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/src/reset_moving_turtle_service.cpp b/software_training_assignment/uwrt_software_trainining/software_training/src/reset_moving_turtle_service.cpp new file mode 100644 index 0000000..28ceadb --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/src/reset_moving_turtle_service.cpp @@ -0,0 +1,67 @@ +#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_coordinates::x; + client_request->y = reset_coordinates::y; + client_request->theta = 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 Resetting"); + response->success = true; +} + +} // namespace composition + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(composition::reset_moving_turtle_service) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/src/spawn_turtle_nodelet.cpp b/software_training_assignment/uwrt_software_trainining/software_training/src/spawn_turtle_nodelet.cpp new file mode 100644 index 0000000..5fa3efc --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/src/spawn_turtle_nodelet.cpp @@ -0,0 +1,64 @@ +#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 a client that makes a request 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() { + + this->timer->cancel(); + + 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 + auto request = std::make_unique(); + + // fill in response + 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 + auto callback = + [this](rclcpp::Client::SharedFuture response) + -> void { + RCLCPP_INFO(this->get_logger(), "Turtle Created: %s", + response.get()->name.c_str()); + }; + + // send request + auto result = client->async_send_request(std::move(request), callback); + } +} + +} // namespace composition + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(composition::spawn_turtle_nodelet) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/src/turtle_neutralize.cpp b/software_training_assignment/uwrt_software_trainining/software_training/src/turtle_neutralize.cpp new file mode 100644 index 0000000..65c590f --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/src/turtle_neutralize.cpp @@ -0,0 +1,47 @@ +#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) { + client = this->create_client("/kill"); + timer = this->create_wall_timer( + 2s, std::bind(&turtle_service_request_node::kill, this)); +} + +void turtle_service_request_node::kill() { + + this->timer->cancel(); + + 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; + + auto callback = + [this](rclcpp::Client::SharedFuture response) + -> void { + (void)response; + RCLCPP_INFO(this->get_logger(), "Turtle Killed"); + }; + + auto result = client->async_send_request(request, callback); + } +} + +} // namespace composition + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(composition::turtle_service_request_node) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/src/turtle_publisher.cpp b/software_training_assignment/uwrt_software_trainining/software_training/src/turtle_publisher.cpp new file mode 100644 index 0000000..a88c221 --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/src/turtle_publisher.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace composition { + +turtle_publisher::turtle_publisher(const rclcpp::NodeOptions &options) + : Node("turtle_publisher", options) { + + auto stationary_turt_callback = + [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this->x_stationary_turt = msg->x; + this->y_stationary_turt = msg->y; + }; + + auto moving_turt_callback = + [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this->x_moving_turt = msg->x; + this->y_moving_turt = msg->y; + }; + + auto publisher_callback = [this](void) -> void { + double position_x{std::abs(this->x_stationary_turt - this->x_moving_turt)}; + double position_y{std::abs(this->y_stationary_turt - this->y_moving_turt)}; + + auto msg = std::make_unique(); + msg->x_pos = position_x; + msg->y_pos = position_y; + msg->distance = std::sqrt((position_x * position_x) + (position_y * position_y)); + + this->publisher->publish(std::move(msg)); + }; + + callbacks = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto callback_option = rclcpp::SubscriptionOptions(); + callback_option.callback_group = callbacks; + + callback_option.topic_stats_options.state = + rclcpp::TopicStatisticsState::Enable; + + std::string s_name("/statistics/"); + std::string node_name(this->get_name()); + std::string stat_name = s_name + node_name; + callback_option.topic_stats_options.publish_topic = stat_name.c_str(); + + 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); + + publisher = this->create_publisher( + "/difference", QUEUE); + + timer = this->create_wall_timer(3s, publisher_callback, callbacks); +} + +} // namespace composition + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(composition::turtle_publisher) \ No newline at end of file diff --git a/software_training_assignment/uwrt_software_trainining/software_training/srv/Software.srv b/software_training_assignment/uwrt_software_trainining/software_training/srv/Software.srv new file mode 100644 index 0000000..d857c6a --- /dev/null +++ b/software_training_assignment/uwrt_software_trainining/software_training/srv/Software.srv @@ -0,0 +1,3 @@ + +--- +bool success diff --git a/three_turtles.png b/three_turtles.png new file mode 100644 index 0000000..844edcd Binary files /dev/null and b/three_turtles.png differ diff --git a/turtle_move.png b/turtle_move.png new file mode 100644 index 0000000..3fee998 Binary files /dev/null and b/turtle_move.png differ