Evan UWRT Challenge

.gitignore

@@ -0,0 +1,4 @@
+/build
+/install
+/log
+/.vscode

README.md

@@ -1 +1,28 @@
-# software_challenge
+<<<<<<< HEAD
+The training challenge consists of the following:
+
+That training relies heavily on components - as this is the newer and better way to go about ROS2 design
+
+Write 6 components that do the following: (Note: Some of the components do not need to be made in the following order)
+
+    Clears any existing turtles
+
+    Create a component that moves 'turtle1' in a circular motion
+
+    Spawns a turtle named "stationary_turtle" at x = 5, y = 5 Spawns a second turtle named "moving_turtle" at x = 25, y = 10
+
+    Create a service that resets the "moving_turtle" to its starting position. The service response should be whether or not it was successful.
+
+    Create a publisher that publishes a custom msg. This custom msgs should have 3 float fields that correspond with the x and y distances of "stationary_turtle" to "moving turtle", as well as the distance between the two turtles.
+
+    Create an action that moves "moving_turtle" to a waypoint in a straight line by publishing geometry_msgs/Twist msgs to turtleX/cmd_vel.The action's goal command is an absolute position of the waypoint, feedback is distance to the goal, and result is the time it took to reach the goal. You should define a custom action file.
+
+Lastly, create a launch file that will start up all the components and the turtlesim node (configure the parameters of the turtlesim node to however you see fit). Ensure that the turtlesim node is launched first as the other components are dependent upon it.
+
+
+After each package, build with `colcon build`, then run the particular service:
+e.g `ros2 run challenge_package <package_name>`, then call the service with `ros2 service call /<service_call_name> std_srvs/srv/Trigger` (only if its a service)
+
+=======
+# software_challenge
+>>>>>>> upstream/master

src/challenge_package/CMakeLists.txt

@@ -0,0 +1,91 @@
+# compiles C++ packages
+cmake_minimum_required(VERSION 3.8)
+project(challenge_package)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(turtlesim REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# need to import msg files like this (from tutorial of https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Single-Package-Define-And-Use-Interface.html)
+set(msg_files "msg/Names.msg")
+set(act_files "action/Move.action")
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    ${msg_files}
+    ${act_files}
+)
+
+include_directories(include)
+ament_export_dependencies(rosidl_default_runtime)
+
+
+add_executable(clear_turtle_listener src/clear_turtles.cpp)
+add_executable(circle_turtle_service src/move_circle.cpp)
+add_executable(spawn_turtle_service src/spawn_turtles.cpp)
+add_executable(reset_turtle_service src/reset_turtle.cpp)
+add_executable(dist_publisher src/publisher_dist.cpp)
+add_executable(move_action_server src/move_action_server.cpp)
+add_executable(move_action_client src/move_action_client.cpp)
+
+# bc msg folder in the same package, so you need to link them with this
+rosidl_target_interfaces(clear_turtle_listener ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rosidl_target_interfaces(move_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+rosidl_target_interfaces(move_action_client ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# check how msg files are imported here and do the same for action files
+
+
+
+# link the dependencies of clear_turtle_listener with relevant libraries that are mentioned in the file itself
+ament_target_dependencies(clear_turtle_listener rclcpp std_msgs turtlesim)
+ament_target_dependencies(circle_turtle_service rclcpp std_srvs geometry_msgs)
+ament_target_dependencies(spawn_turtle_service rclcpp std_srvs turtlesim)
+ament_target_dependencies(reset_turtle_service rclcpp std_srvs turtlesim)
+ament_target_dependencies(dist_publisher rclcpp geometry_msgs turtlesim)
+ament_target_dependencies(move_action_server rclcpp rclcpp_action geometry_msgs turtlesim)
+ament_target_dependencies(move_action_client rclcpp rclcpp_action geometry_msgs turtlesim)
+
+
+
+install(TARGETS
+  clear_turtle_listener
+  circle_turtle_service
+  spawn_turtle_service
+  reset_turtle_service
+  dist_publisher
+  move_action_server
+  move_action_client
+  DESTINATION lib/${PROJECT_NAME})
+
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/challenge_package/action/Move.action

@@ -0,0 +1,9 @@
+# Define the action goal
+float32 x_pos
+float32 y_pos
+---
+# Define the action result
+float32 time_taken
+---
+# Define the action feedback
+float32 distance_to_goal

src/challenge_package/include/challenge_package/common.hpp

@@ -0,0 +1,11 @@
+#ifndef COMMON_H
+#define COMMON_H
+
+// move these to a central file and import
+struct Position {
+  float x;
+  float y;
+  float theta;
+};
+
+#endif

src/challenge_package/launch/launch.py

@@ -0,0 +1,41 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='turtlesim',
+            namespace='',
+            executable='turtlesim_node',
+        ),
+        Node(
+            package='challenge_package',
+            namespace='',
+            executable='clear_turtle_listener',
+        ),
+        Node(
+            package='challenge_package',
+            namespace='',
+            executable='circle_turtle_service',
+        ),
+        Node(
+            package='challenge_package',
+            namespace='',
+            executable='spawn_turtle_service',
+        ),
+        Node(
+            package='challenge_package',
+            namespace='',
+            executable='reset_turtle_service',
+        ),
+        Node(
+            package='challenge_package',
+            namespace='',
+            executable='dist_publisher',
+        ),
+        Node(
+            package='challenge_package',
+            namespace='',
+            executable='move_action_server',
+        ),
+    ])

src/challenge_package/msg/Names.msg

@@ -0,0 +1 @@
+string[] names

src/challenge_package/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>challenge_package</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">yaybeno</maintainer>
+  <license>TODO: License declaration</license>
+<!-- compiles stuff from ROS -->
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  <depend>turtlesim</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>rcutils</depend>
+  <depend>rcl</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/challenge_package/src/clear_turtles.cpp

@@ -0,0 +1,46 @@
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "turtlesim/srv/kill.hpp"
+#include "challenge_package/msg/names.hpp"
+
+using std::placeholders::_1;
+
+class KillTurtlesSubscriber : public rclcpp::Node 
+{
+  public:
+    KillTurtlesSubscriber()
+    : Node("kill_turtle_subscriber")
+    {
+      client = create_client<turtlesim::srv::Kill>("/kill");
+      subscription_ = this->create_subscription<challenge_package::msg::Names>(
+      "kill_turtles", 10, std::bind(&KillTurtlesSubscriber::topic_callback, this, _1));
+    }
+
+  private:
+    rclcpp::Client<turtlesim::srv::Kill>::SharedPtr client;
+    void topic_callback(const challenge_package::msg::Names& msg) const
+    {     
+      for (unsigned int i = 0; i < sizeof(msg); i++) {
+        const std::string name = msg.names[i];
+
+        // auto is for typing anything with a complex type
+        auto kill_req = std::make_shared<turtlesim::srv::Kill::Request>();
+        kill_req->name = name;
+        auto res = client->async_send_request(kill_req);
+
+        RCLCPP_INFO(this->get_logger(), "Killed Turtle: '%s'", name.c_str());
+      }
+    }
+    rclcpp::Subscription<challenge_package::msg::Names>::SharedPtr subscription_; // sharedptr will share stuff by memory, but no big advantage (allegedly)
+
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<KillTurtlesSubscriber>());
+  rclcpp::shutdown();
+  return 0;
+}

src/challenge_package/src/move_action_client.cpp

@@ -0,0 +1,98 @@
+#include <memory>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "challenge_package/action/move.hpp"
+
+using move_action = challenge_package::action::Move;
+using goal_handle_move = rclcpp_action::ClientGoalHandle<move_action>;
+
+class MoveActionClient : public rclcpp::Node 
+{
+public:
+  MoveActionClient()
+  : Node("move_action_client")
+  {
+    this->client_ptr = rclcpp_action::create_client<move_action>(
+      this,
+      "move_action"
+    );
+
+    // Wait for action server to be available
+    while (!client_ptr->wait_for_action_server(std::chrono::seconds(5))) {
+      RCLCPP_INFO(this->get_logger(), "Action server not available. Waiting...");
+    }
+
+    this->send_goal();
+  }
+
+private:
+  rclcpp_action::Client<move_action>::SharedPtr client_ptr;
+
+  void send_goal() 
+  {
+    auto goal_msg = move_action::Goal();
+    goal_msg.x_pos = 10.0;  // For demonstration purposes
+    goal_msg.y_pos = 7.5;
+
+    RCLCPP_INFO(this->get_logger(), "Sending goal to x: %f, y: %f", goal_msg.x_pos, goal_msg.y_pos);
+
+    auto send_goal_options = rclcpp_action::Client<move_action>::SendGoalOptions();
+
+    send_goal_options.goal_response_callback = 
+      std::bind(&MoveActionClient::goal_response_callback, this, std::placeholders::_1);
+
+    send_goal_options.feedback_callback = 
+      std::bind(&MoveActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
+
+    send_goal_options.result_callback = 
+      std::bind(&MoveActionClient::result_callback, this, std::placeholders::_1);
+
+    this->client_ptr->async_send_goal(goal_msg, send_goal_options);
+  }
+
+  void goal_response_callback(std::shared_future<goal_handle_move::SharedPtr> future)
+  {
+    auto goal_handle = future.get();
+    if (!goal_handle) {
+      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
+    } else {
+      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
+    }
+  }
+
+  void feedback_callback(
+    goal_handle_move::SharedPtr,
+    const std::shared_ptr<const move_action::Feedback> feedback)
+  {
+    RCLCPP_INFO(this->get_logger(), "Distance to goal: %f", feedback->distance_to_goal);
+  }
+
+  void result_callback(const goal_handle_move::WrappedResult &result)
+  {
+    switch(result.code) {
+      case rclcpp_action::ResultCode::SUCCEEDED:
+        RCLCPP_INFO(this->get_logger(), "Goal succeeded! Time taken: %f", result.result->time_taken);
+        break;
+      case rclcpp_action::ResultCode::ABORTED:
+        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+        return;
+      case rclcpp_action::ResultCode::CANCELED:
+        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+        return;
+      default:
+        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+        return;
+    }
+  }
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto client_node = std::make_shared<MoveActionClient>();
+  rclcpp::spin(client_node);
+  rclcpp::shutdown();
+  return 0;
+}