software_training_assignment

.gitignore

@@ -0,0 +1 @@
+turtlesim/

README.md

@@ -1 +1,24 @@
-# software_challenge
+# software_challenge
+UWRT Software Training Assignment done using ROS2
+
+## Instructions
+1. Create a ros package called software_training_assignment
+2. Install the turtlesim package(hint: you shouldn't be downloading any source code from github. You should be using a package manager).
+3. Create a launch file that launches the turtlesim node with a background colour of (r=128, b=0, g=128)
+4. Write a node that:
+    a. Clears any existing turtles
+    b. Spawns a turtle named "stationary_turtle" at x = 5, y = 5
+    c. Spawns a second turtle named "moving_turtle" at x = 25, y = 10
+    d. Create a service that resets the "moving_turtle" to its starting position. 
+        The service response should be whether or not it was successful.
+    e. Create a publisher that publishes a custom msg. 
+        This custom msgs should have 3 integer fields that correspond with the x and y distances of "stationary_turtle" to "moving turtle", 
+        as well as the distance between the two turtles.
+    f. 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 cd 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.
+5. Add your node you created to your launch file. Make sure your node doesn't crash if the turtlesim_node has not come up yet.
+6. Create a .gitignore file so that only relevant files are committed. The point of this is to
+avoid committing all of your IDE specific configuration files.
+7. Create a fork of the https://github.com/uwrobotics/software_challenge​ and push your
+package to your fork. Make a pr to the original. The root directory of the repo should only contain your package folder.

action_tutorials_cpp/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(action_tutorials_cpp)
+
+# 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(action_tutorials_interfaces REQUIRED)
+find_package(motion_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(turtlesim REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+include_directories(include)
+
+add_library(action_tutorials SHARED
+  src/waypoint_action_server.cpp)
+
+rclcpp_components_register_node(action_tutorials PLUGIN "action_tutorials_cpp::WaypointActionServer" EXECUTABLE waypoint_action_server)
+target_compile_definitions(action_tutorials
+  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
+ament_target_dependencies(action_tutorials
+  "action_tutorials_interfaces"
+  "rclcpp"
+  "rclcpp_action"
+  "motion_interfaces"
+  "rclcpp_components"
+  "turtlesim"
+  "geometry_msgs")
+
+install(TARGETS
+  action_tutorials
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h

@@ -0,0 +1,58 @@
+// Copyright 2019 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 ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
+#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_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 ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
+    #define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
+  #else
+    #define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
+    #define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
+    #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
+  #else
+    #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
+  #endif
+  #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
+  #define ACTION_TUTORIALS_CPP_LOCAL
+#else
+  #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
+  #define ACTION_TUTORIALS_CPP_IMPORT
+  #if __GNUC__ >= 4
+    #define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
+    #define ACTION_TUTORIALS_CPP_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define ACTION_TUTORIALS_CPP_PUBLIC
+    #define ACTION_TUTORIALS_CPP_LOCAL
+  #endif
+  #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_

action_tutorials_cpp/package.xml

@@ -0,0 +1,23 @@
+<?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>action_tutorials_cpp</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">Abdur</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+
+  <depend>motion_interfaces</depend>
+  <depend>turtlesim</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

action_tutorials_cpp/src/waypoint_action_server.cpp

@@ -0,0 +1,184 @@
+// Copyright 2019 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.
+
+#include <memory>
+
+#include "action_tutorials_interfaces/action/fibonacci.hpp"
+#include "rclcpp/rclcpp.hpp"
+// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "action_tutorials_cpp/visibility_control.h"
+#include "motion_interfaces/action/go_to_waypoint.hpp"
+
+#include <geometry_msgs/msg/twist.hpp>
+#include "turtlesim/msg/pose.hpp"
+
+
+namespace action_tutorials_cpp
+{
+class WaypointActionServer : public rclcpp::Node
+{
+public:
+  using Waypoint = motion_interfaces::action::GoToWaypoint;
+  using GoalHandleWaypoint = rclcpp_action::ServerGoalHandle<Waypoint>;
+
+  ACTION_TUTORIALS_CPP_PUBLIC
+  explicit WaypointActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+  : Node("fibonacci_action_server", options)
+  {
+    using namespace std::placeholders;
+    RCLCPP_INFO(this->get_logger(), "Starting Waypoint Action Server 2!");
+
+    pose_ = this->create_subscription<turtlesim::msg::Pose>("/moving_turtle/pose", 10, std::bind(&WaypointActionServer::poseCallback, this, _1));
+    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/moving_turtle/cmd_vel", 10);
+
+
+    this->action_server_ = rclcpp_action::create_server<Waypoint>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      "waypoint",
+      std::bind(&WaypointActionServer::handle_goal, this, _1, _2),
+      std::bind(&WaypointActionServer::handle_cancel, this, _1),
+      std::bind(&WaypointActionServer::handle_accepted, this, _1));
+  }
+
+private:
+  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_;
+  float poseX = 0;
+  float poseY = 0;
+  float poseTheta = 0;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_; 
+  rclcpp_action::Server<Waypoint>::SharedPtr action_server_;
+
+  void poseCallback(const turtlesim::msg::Pose::SharedPtr msg)
+  {
+      poseX = msg->x;
+      poseY = msg->y;
+      poseTheta = msg->theta;
+  } 
+
+  ACTION_TUTORIALS_CPP_LOCAL
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const Waypoint::Goal> goal)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received goal request");
+    (void)uuid;
+    // Let's reject sequences that are over 9000
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  ACTION_TUTORIALS_CPP_LOCAL
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<GoalHandleWaypoint> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  ACTION_TUTORIALS_CPP_LOCAL
+  void handle_accepted(const std::shared_ptr<GoalHandleWaypoint> goal_handle)
+  {
+    using namespace std::placeholders;
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&WaypointActionServer::execute, this, _1), goal_handle}.detach();
+  }
+
+  ACTION_TUTORIALS_CPP_LOCAL
+  void execute(const std::shared_ptr<GoalHandleWaypoint> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Executing Goal");
+    rclcpp::Rate loop_rate(std::chrono::milliseconds(100));
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<Waypoint::Feedback>();
+    geometry_msgs::msg::Twist cmd_vel;
+    auto result = std::make_shared<Waypoint::Result>();
+
+    auto start = rclcpp::Node::now();
+    float targX = goal->x;
+    float targY = goal->y;
+
+    double kP_linear = 0.5;
+    double kP_angular = 2;
+    double tolerance = 0.01;
+
+    while(rclcpp::ok()){
+        if(goal_handle->is_canceling()){
+            auto diff = rclcpp::Node::now() - start;
+            result->time = diff.seconds();
+
+            goal_handle->canceled(result);
+            RCLCPP_INFO(this->get_logger(), "Goal canceled");
+            return;
+        }
+
+        float dist = getDist(targX, poseX, targY, poseY);
+        float angleGoal = atan2((targY-poseY),(targX-poseX));
+
+        if(abs(angleGoal-poseTheta) > tolerance){
+            cmd_vel.angular.z = kP_angular * (angleGoal-poseTheta);
+            cmd_vel.angular.x = 0;
+            cmd_vel.angular.y = 0;
+            cmd_vel.linear.x = 0;
+            cmd_vel.linear.y = 0;
+            cmd_vel.linear.z = 0;
+            publisher_->publish(cmd_vel);
+            feedback->dist = dist;
+            goal_handle->publish_feedback(feedback);
+            RCLCPP_INFO(this->get_logger(), "Reaching Angle Goal %.2f current Theta: %.2f", angleGoal, poseTheta);
+
+            loop_rate.sleep();
+            continue;
+        }else if(dist > tolerance){
+            cmd_vel.angular.z = 0;
+            cmd_vel.angular.x = 0;
+            cmd_vel.angular.y = 0;
+
+            cmd_vel.linear.x = kP_linear * dist;
+            cmd_vel.linear.y = 0;
+            cmd_vel.linear.z = 0;
+
+            publisher_->publish(cmd_vel);
+            feedback->dist = dist;
+            goal_handle->publish_feedback(feedback);
+            RCLCPP_INFO(this->get_logger(), "Publish feedback");
+
+            loop_rate.sleep();
+            continue;
+        }else{
+            break;
+        }
+    }
+    if(rclcpp::ok()){
+        auto diff = rclcpp::Node::now() - start;
+        result->time = diff.seconds();
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }               
+  }
+
+  static float getDist(float x1, float x2, float y1, float y2){
+      return sqrt(pow(x2-x1,2) + pow(y2-y1,2));
+  }  
+
+};  // class WaypointActionServer
+
+}  // namespace action_tutorials_cpp
+
+RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::WaypointActionServer)

motion_interfaces/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.5)
+project(motion_interfaces)
+
+# 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(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/TurtleDistance.msg"
+  "action/GoToWaypoint.action"
+  "srv/ResetService.srv"
+ )
+
+
+ament_package()

motion_interfaces/action/GoToWaypoint.action

@@ -0,0 +1,6 @@
+int64 x
+int64 y
+---
+float64 time
+---
+float64 dist

motion_interfaces/msg/TurtleDistance.msg

@@ -0,0 +1,3 @@
+int64 x
+int64 y
+float64 dist