diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..412da76
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+turtlesim/
\ No newline at end of file
diff --git a/README.md b/README.md
index 77bcc77..63c9527 100644
--- a/README.md
+++ b/README.md
@@ -1 +1,24 @@
-# software_challenge
\ No newline at end of file
+# 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.
diff --git a/action_tutorials_cpp/CMakeLists.txt b/action_tutorials_cpp/CMakeLists.txt
new file mode 100644
index 0000000..c2f585d
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h b/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h
new file mode 100644
index 0000000..5eaa1f9
--- /dev/null
+++ b/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_
\ No newline at end of file
diff --git a/action_tutorials_cpp/package.xml b/action_tutorials_cpp/package.xml
new file mode 100644
index 0000000..b58ab78
--- /dev/null
+++ b/action_tutorials_cpp/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ action_tutorials_cpp
+ 0.0.0
+ TODO: Package description
+ Abdur
+ TODO: License declaration
+
+ ament_cmake
+
+
+ motion_interfaces
+ turtlesim
+ geometry_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/action_tutorials_cpp/src/waypoint_action_server.cpp b/action_tutorials_cpp/src/waypoint_action_server.cpp
new file mode 100644
index 0000000..8259786
--- /dev/null
+++ b/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
+
+#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
+#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;
+
+ 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("/moving_turtle/pose", 10, std::bind(&WaypointActionServer::poseCallback, this, _1));
+ publisher_ = this->create_publisher("/moving_turtle/cmd_vel", 10);
+
+
+ this->action_server_ = rclcpp_action::create_server(
+ 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::SharedPtr pose_;
+ float poseX = 0;
+ float poseY = 0;
+ float poseTheta = 0;
+ rclcpp::Publisher::SharedPtr publisher_;
+ rclcpp_action::Server::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 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 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 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 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();
+ geometry_msgs::msg::Twist cmd_vel;
+ auto result = std::make_shared();
+
+ 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)
\ No newline at end of file
diff --git a/motion_interfaces/CMakeLists.txt b/motion_interfaces/CMakeLists.txt
new file mode 100644
index 0000000..a9a7829
--- /dev/null
+++ b/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()
diff --git a/motion_interfaces/action/GoToWaypoint.action b/motion_interfaces/action/GoToWaypoint.action
new file mode 100644
index 0000000..541af22
--- /dev/null
+++ b/motion_interfaces/action/GoToWaypoint.action
@@ -0,0 +1,6 @@
+int64 x
+int64 y
+---
+float64 time
+---
+float64 dist
\ No newline at end of file
diff --git a/motion_interfaces/msg/TurtleDistance.msg b/motion_interfaces/msg/TurtleDistance.msg
new file mode 100644
index 0000000..e8d0f06
--- /dev/null
+++ b/motion_interfaces/msg/TurtleDistance.msg
@@ -0,0 +1,3 @@
+int64 x
+int64 y
+float64 dist
\ No newline at end of file
diff --git a/motion_interfaces/package.xml b/motion_interfaces/package.xml
new file mode 100644
index 0000000..957d6f1
--- /dev/null
+++ b/motion_interfaces/package.xml
@@ -0,0 +1,25 @@
+
+
+
+ motion_interfaces
+ 0.0.0
+ TODO: Package description
+ Abdur
+ TODO: License declaration
+
+ ament_cmake
+ rosidl_default_generators
+
+ action_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+ rosidl_default_generators
+ rosidl_default_generators
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
diff --git a/motion_interfaces/srv/ResetService.srv b/motion_interfaces/srv/ResetService.srv
new file mode 100644
index 0000000..1308cc2
--- /dev/null
+++ b/motion_interfaces/srv/ResetService.srv
@@ -0,0 +1,2 @@
+---
+bool success
\ No newline at end of file
diff --git a/onboarding_assignment/CMakeLists.txt b/onboarding_assignment/CMakeLists.txt
new file mode 100644
index 0000000..adeeea8
--- /dev/null
+++ b/onboarding_assignment/CMakeLists.txt
@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 3.5)
+project(onboarding_assignment)
+
+# 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(motion_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(turtlesim REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+add_executable(distance_publisher src/DistancePublisher.cpp)
+ament_target_dependencies(distance_publisher rclcpp turtlesim motion_interfaces)
+
+add_executable(main_node src/MainNode.cpp)
+ament_target_dependencies(main_node rclcpp rclcpp_action rclcpp_components turtlesim motion_interfaces)
+
+add_executable(server src/ResetServer.cpp)
+ament_target_dependencies(server
+rclcpp motion_interfaces turtlesim)
+
+install(TARGETS
+ distance_publisher
+ main_node
+ server
+ DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
diff --git a/onboarding_assignment/include/onboarding_assignment/visiblity_control.h b/onboarding_assignment/include/onboarding_assignment/visiblity_control.h
new file mode 100644
index 0000000..5eaa1f9
--- /dev/null
+++ b/onboarding_assignment/include/onboarding_assignment/visiblity_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_
\ No newline at end of file
diff --git a/onboarding_assignment/package.xml b/onboarding_assignment/package.xml
new file mode 100644
index 0000000..9746402
--- /dev/null
+++ b/onboarding_assignment/package.xml
@@ -0,0 +1,25 @@
+
+
+
+ onboarding_assignment
+ 0.0.0
+ TODO: Package description
+ Abdur
+ TODO: License declaration
+
+ ament_cmake
+
+ motion_interfaces
+ turtlesim
+ geometry_msgs
+ rclcpp
+ rclcpp_action
+ rclcpp_components
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/onboarding_assignment/src/DistancePublisher.cpp b/onboarding_assignment/src/DistancePublisher.cpp
new file mode 100644
index 0000000..6d60be1
--- /dev/null
+++ b/onboarding_assignment/src/DistancePublisher.cpp
@@ -0,0 +1,66 @@
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "motion_interfaces/msg/turtle_distance.hpp" // CHANGE
+#include "turtlesim/msg/pose.hpp"
+
+#include
+#include
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+class DistancePublisher : public rclcpp::Node{
+ public:
+ DistancePublisher(int dt, std::string pose1_topic, std::string pose2_topic) : Node("distance_publisher") {
+ pose1_ = this->create_subscription(pose1_topic, 10, std::bind(&DistancePublisher::pose1Callback, this, _1));
+ pose2_ = this->create_subscription(pose2_topic, 10, std::bind(&DistancePublisher::pose2Callback, this, _1));
+
+ publisher_ = this->create_publisher("distance", 10);
+ timer_ = this->create_wall_timer(
+ std::chrono::milliseconds(dt), std::bind(&DistancePublisher::timer_callback, this)
+ );
+ }
+
+ private:
+ // Pose 1 Subscription
+ rclcpp::Subscription::SharedPtr pose1_;
+ float pose1_X = 0;
+ float pose1_Y = 0;
+ // Pose 2 Subscription
+ rclcpp::Subscription::SharedPtr pose2_;
+ float pose2_X = 0;
+ float pose2_Y = 0;
+
+ // Publisher
+ rclcpp::Publisher::SharedPtr publisher_;
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ void pose1Callback(const turtlesim::msg::Pose::SharedPtr msg){
+ pose1_X = msg->x;
+ pose1_Y = msg->y;
+ }
+ void pose2Callback(const turtlesim::msg::Pose::SharedPtr msg){
+ pose2_X = msg->x;
+ pose2_Y = msg->y;
+ }
+
+ void timer_callback(){
+ auto message = motion_interfaces::msg::TurtleDistance();
+ message.x = std::abs(pose1_X-pose2_X);
+ message.y = std::abs(pose1_Y-pose2_Y);
+ message.dist = sqrt(pow(pose1_X-pose2_X,2)+pow(pose1_Y-pose2_Y,2));
+
+ //RCLCPP_INFO(this->get_logger(), "Publishing: dX: '%d', dY: '%d', dist: '%.2f'", message.x, message.y, message.dist);
+ publisher_->publish(message);
+ }
+};
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared(500, "/moving_turtle/pose", "/stationary_turtle/pose"));
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/onboarding_assignment/src/MainNode.cpp b/onboarding_assignment/src/MainNode.cpp
new file mode 100644
index 0000000..65cf8bc
--- /dev/null
+++ b/onboarding_assignment/src/MainNode.cpp
@@ -0,0 +1,177 @@
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "turtlesim/srv/spawn.hpp"
+#include "turtlesim/srv/kill.hpp"
+#include "motion_interfaces/msg/turtle_distance.hpp"
+
+#include "motion_interfaces/action/go_to_waypoint.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+
+//TODO: Remove hardcoded waypoint
+class MainNode : public rclcpp::Node{
+ public:
+ using Waypoint = motion_interfaces::action::GoToWaypoint;
+ using GoalHandleWaypoint = rclcpp_action::ClientGoalHandle;
+
+ MainNode() : Node("main_node"){
+ this->clear();
+ this->spawn("stationary_turtle", 5.0, 5.0, 0.0);
+ this->spawn("moving_turtle", 7.0, 10.0, 0.0);
+ distance_ = this->create_subscription("distance", 10, std::bind(&MainNode::distanceCallback, this, _1));
+
+ this->client_ptr_ = rclcpp_action::create_client(this, "waypoint");
+
+ this->timer_ = this->create_wall_timer(
+ 1000ms, std::bind(&MainNode::send_goal, this));
+
+ }
+
+ void clear(){
+ std::shared_ptr node = rclcpp::Node::make_shared("kill_turtle_client");
+ rclcpp::Client::SharedPtr client =
+ node->create_client("/kill");
+
+ auto request = std::make_shared();
+ request->name = "turtle1";
+
+ while (!client->wait_for_service(1s)) {
+ if (!rclcpp::ok()) {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
+ return;
+ }
+ 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"), "Killed Turtle");
+ } else {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to kill turtle");
+ }
+ }
+
+ void spawn(std::string turtleName, float x, float y, float theta){
+ std::shared_ptr node = rclcpp::Node::make_shared("spawn_turtle_client");
+ rclcpp::Client::SharedPtr client =
+ node->create_client("/spawn");
+
+ auto request = std::make_shared();
+ request->name = turtleName;
+ request->x = x;
+ request->y = y;
+ request->theta = theta;
+
+ while (!client->wait_for_service(1s)) {
+ if (!rclcpp::ok()) {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
+ return;
+ }
+ 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"), "Spawned turtle %s", result.get()->name);
+ } else {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to Spawn Turtle");
+ }
+ }
+
+ private:
+ rclcpp_action::Client::SharedPtr client_ptr_;
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ rclcpp::Subscription::SharedPtr distance_;
+
+ void send_goal(){
+ using namespace std::placeholders;
+ this->timer_->cancel();
+
+ if (!this->client_ptr_->wait_for_action_server()) {
+ RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
+ rclcpp::shutdown();
+ }
+
+ auto goal_msg = Waypoint::Goal();
+ goal_msg.x = 2;
+ goal_msg.y = 2;
+
+ RCLCPP_INFO(this->get_logger(), "Sending goal");
+
+ auto send_goal_options = rclcpp_action::Client::SendGoalOptions();
+
+ send_goal_options.goal_response_callback =
+ std::bind(&MainNode::goal_response_callback, this, _1);
+ send_goal_options.feedback_callback =
+ std::bind(&MainNode::feedback_callback, this, _1, _2);
+ send_goal_options.result_callback =
+ std::bind(&MainNode::result_callback, this, _1);
+
+ this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
+ }
+
+ void goal_response_callback(std::shared_future 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(GoalHandleWaypoint::SharedPtr, const std::shared_ptr feedback)
+ {
+ std::stringstream ss;
+ ss << "Curr Dist: " << feedback->dist << " ";
+ RCLCPP_INFO(this->get_logger(), ss.str().c_str());
+ }
+
+ void result_callback(const GoalHandleWaypoint::WrappedResult & result)
+ {
+ switch (result.code) {
+ case rclcpp_action::ResultCode::SUCCEEDED:
+ 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;
+ }
+ std::stringstream ss;
+ ss << "Result received: " << result.result->time << " ";
+ RCLCPP_INFO(this->get_logger(), ss.str().c_str());
+ rclcpp::shutdown();
+ }
+
+ void distanceCallback(const motion_interfaces::msg::TurtleDistance::SharedPtr msg) const{
+ RCLCPP_INFO(this->get_logger(), "Received: dX: '%d', dY: '%d', dist: '%.1f'", msg->x, msg->y, msg->dist);
+ }
+};
+
+
+int main(int argc, char * argv[]){
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/onboarding_assignment/src/ResetServer.cpp b/onboarding_assignment/src/ResetServer.cpp
new file mode 100644
index 0000000..9d1b354
--- /dev/null
+++ b/onboarding_assignment/src/ResetServer.cpp
@@ -0,0 +1,56 @@
+#include "rclcpp/rclcpp.hpp"
+#include "motion_interfaces/srv/reset_service.hpp"
+#include "turtlesim/srv/teleport_absolute.hpp"
+
+using namespace std::chrono_literals;
+
+void reset(const std::shared_ptr request,
+ std::shared_ptr response){
+
+ const float x = 5.55;
+ const float y = 5.55;
+ const float theta = 0.0;
+
+ std::shared_ptr node = rclcpp::Node::make_shared("reset_client");
+ rclcpp::Client::SharedPtr client =
+ node->create_client("/moving_turtle/teleport_absolute");
+
+ auto teleport_request = std::make_shared();
+ teleport_request->x = x;
+ teleport_request->y = y;
+ teleport_request->theta = theta;
+
+ while (!client->wait_for_service(1s)) {
+ if (!rclcpp::ok()) {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
+ return;
+ }
+ RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
+ }
+
+ auto result = client->async_send_request(teleport_request);
+ if (rclcpp::spin_until_future_complete(node, result) ==
+ rclcpp::FutureReturnCode::SUCCESS)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: true");
+ response->success = true;
+ } else {
+ RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: true");
+ response->success = false;
+ }
+
+}
+
+int main(int argc, char **argv)
+{
+ rclcpp::init(argc, argv);
+ RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "init server");
+
+ std::shared_ptr node = rclcpp::Node::make_shared("reset_server");
+
+ rclcpp::Service::SharedPtr service =
+ node->create_service("reset_turtle", &reset);
+
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+}
\ No newline at end of file
diff --git a/turtlesim_launch.py b/turtlesim_launch.py
new file mode 100644
index 0000000..0b98f6c
--- /dev/null
+++ b/turtlesim_launch.py
@@ -0,0 +1,29 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='turtlesim',
+ executable='turtlesim_node',
+ parameters=[
+ {"background_r": 128},
+ {"background_g": 128},
+ {"background": 0}
+ ]
+ ),
+ Node(
+ package='onboarding_assignment',
+ executable='main_node'
+ ),
+ Node(
+ package='onboarding_assignment',
+ executable='distance_publisher'),
+ Node(
+ package='onboarding_assignment',
+ executable='server'),
+ Node(
+ package='action_tutorials_cpp',
+ executable='waypoint_action_server')
+
+ ])
\ No newline at end of file