diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..b05db73
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+CMakeLists.txt
+!software_training_assignment/CMakeLists.txt
+/ros_notes
diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt
new file mode 100644
index 0000000..1f7cbd2
--- /dev/null
+++ b/software_training_assignment/CMakeLists.txt
@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(software_training_assignment)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+ turtlesim
+ message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+ FILES
+ turtleDistances.msg
+ )
+
+## Generate services in the 'srv' folder
+ add_service_files(
+ FILES
+ resetTurtle.srv
+)
+
+## Generate actions in the 'action' folder
+ add_action_files(
+ FILES
+ moveTurtle.action
+ )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES software_training_assignment
+ CATKIN_DEPENDS
+ roscpp
+ rospy
+ std_msgs
+ message_runtime
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/software_training_assignment.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+ add_executable(${PROJECT_NAME}_node src/main.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+ add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(${PROJECT_NAME}_node
+ ${catkin_LIBRARIES}
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_software_training_assignment.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/software_training_assignment/action/moveTurtle.action b/software_training_assignment/action/moveTurtle.action
new file mode 100644
index 0000000..326ac3f
--- /dev/null
+++ b/software_training_assignment/action/moveTurtle.action
@@ -0,0 +1,9 @@
+# Action goal
+int64 abs_x
+int64 abs_y
+---
+# Action Feedback
+int64 abs_distance
+---
+# Action result (time in sec)
+int64 time
\ No newline at end of file
diff --git a/software_training_assignment/launch/turtlesim.launch b/software_training_assignment/launch/turtlesim.launch
new file mode 100755
index 0000000..6c6292a
--- /dev/null
+++ b/software_training_assignment/launch/turtlesim.launch
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/software_training_assignment/msg/turtleDistances.msg b/software_training_assignment/msg/turtleDistances.msg
new file mode 100644
index 0000000..68d3290
--- /dev/null
+++ b/software_training_assignment/msg/turtleDistances.msg
@@ -0,0 +1,3 @@
+int64 xDistance # distance between x coordinates of the stationary and moving turtles
+int64 yDistance # distance between y coordinates of the stationary and moving turtles
+int64 distance # distance betweent the two turtles
diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml
new file mode 100644
index 0000000..b792526
--- /dev/null
+++ b/software_training_assignment/package.xml
@@ -0,0 +1,72 @@
+
+
+ software_training_assignment
+ 0.0.0
+ The software_training_assignment package
+
+
+
+
+ urbanpistek
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ turtlesim
+ message_generation
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ turtlesim
+ message_runtime
+
+
+
+
+
+
+
+
diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp
new file mode 100644
index 0000000..cdf8f3f
--- /dev/null
+++ b/software_training_assignment/src/main.cpp
@@ -0,0 +1,111 @@
+/*
+The main client for the node
+*/
+
+#include
+#include
+#include
+
+//Service for turtlesim spawn
+#include
+
+//Service to clear the turtles
+//(I am guessing it clears the turtles)
+//Based on the wording of the documentation
+#include
+
+//Including our custom message
+#include
+
+//Include Action Library
+#include
+
+//Class for determining the distances between the turtles
+class turtleDistance{
+ public:
+ int xd;
+ int yd;
+ int distance;
+ turtleDistance(int t1_x, int t1_y, int t2_x, int t2_y){
+ xd = t1_x - t2_x;
+ yd = t1_y - t2_y;
+ distance = sqrt(xd*xd + yd*yd);
+ }
+}
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "main");
+ ros::NodeHandle nh;
+
+ // Clear Turtles \\
+
+ //Create Client object for the Clear service
+ ros::ServiceClient clearClient = nh.ServiceClient ("clear");
+ //Calls the service, waits for the return
+ bool clearSuccess = clearClient.call();
+ //Checks if the service call was successful
+ if(clearSuccess) {
+ ROS_INFO_STREAM("Turtles Cleared");
+ } else {
+ ROS_ERROR_STREAM("Failed to clear turtles");
+ }
+
+ // Spawn Turtles \\
+
+ //Create Client object for the spawn service
+ ros::ServiceClient spawnClient = nh.ServiceClient ("spawn");
+
+ //Create the spawn request response objects
+ turtlesim::Spawn::Request spawn_req;
+ turtlesim::Spawn::Response spawn_resp;
+
+ //All values to be passed into the service
+ int x_coords[2] = {5, 25};
+ int y_coords[2] = {5, 10};
+ string turtlenames[2] = {"stationary_turtle", "moving_turtle"};
+
+ //Spawns all turtles
+ for(int idx = 0; 0 < 2; idx++){
+ //Fill the request data members
+ spawn_req.x = x_coords[idx];
+ spawn_req.y = y_coords[idx];
+ spawn_req.theta = M_PI/2;
+ spawn_req.name = turtlenames[idx];
+
+ //Calls the service, waits for the return
+ bool spawnSuccess = spawnClient.call(spawn_req, spawn_resp);
+
+ //Checks if the service call was successful
+ if(spawnSuccess) {
+ ROS_INFO_STREAM("Spawned turtle: " << spawn_resp.name);
+ } else {
+ ROS_ERROR_STREAM("Failed to spawn");
+ }
+ }
+
+ //Infinite loop to run
+ while(true){
+ turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]);
+ //Publisher for turtle distances
+ ros::Publisher turtleDist_pub = nh.advertise("turtle_distnaces", 10);
+ turtleDist_pub.publish(turtleDist);
+
+ //Creating a Action Client
+ actionlib::SimpleActionClient ac("moveTurtle", true);
+ ROS_INFO("Waiting for Action Server to start");
+ ac.waitForServer();
+ //Define the waypoint for which the turtle should move to
+ int x = 0;
+ int y = 0;
+ int goal[2] = {x, y};
+ ac.sendGoal(goal);
+
+ bool waitForAction = ac.waitForResult(ros::Duration(30.0));
+ if(waitForAction){
+ ROS_INFO("Action Finished");
+ } else {
+ ROS_INFO("Action Failed");
+ }
+ }
+
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/moveTurtle.cpp b/software_training_assignment/src/moveTurtle.cpp
new file mode 100644
index 0000000..090f002
--- /dev/null
+++ b/software_training_assignment/src/moveTurtle.cpp
@@ -0,0 +1,83 @@
+/*
+Action server for moving the turtle
+*/
+
+#include
+#include
+#include
+//Subscribe to the geometry_msgs/Twist topic
+#include
+#include
+
+//Class to define the action server
+class MoveTurtleAction {
+ protected:
+ ros::NodeHandle nh;
+ actionlib::SimpleActionServer as;
+ std::string action_name;
+ software_training_assignment::moveTurtleFeedback feedback;
+ software_training_assignment::moveTurtleResult result;
+
+ public:
+
+ MoveTurtleAction(std::string name) :
+ as(nh, name, boost::bind(&MoveTurtleAction::executeCB, this, _1), false, action_name(name)) {
+ as.start();
+ }
+
+ ~MoveTurtleAction(void){}
+
+ void executeCB(const software_training_assignment::moveTurtleGoalConstPtr &goal){
+
+ bool success = false;
+ geometry_msgs::Twist vel_msg;
+ ros::Publisher pub = nh.advertise("/turtle1/cmd_vel", 10);
+ int end_pos_x = goal->abs_x;
+ int end_pos_y = goal->abs_y;
+ double distance = sqrt(end_pos_x*end_pos_x + end_pos_y*end_pos_y);
+ double elapsed_dist = 0;
+
+ //start time
+ double t0 = ros::Time::now().toSec();
+ //move turtle to the end position using the unit vector for the velocity
+ vel_msg.linear.x = (end_pos_x/distance);
+ vel_msg.linear.y = (end_pos_y/distance);
+ vel_msg.linear.z = 0;
+ vel_msg.angular.x = 0;
+ vel_msg.angular.y = 0;
+ vel_msg.angular.z = 0;
+
+ //publisher message
+ pub.publish(vel_msg);
+
+ while(elapsed_dist < distance){
+ double t1 = ros::Time::now().toSec();
+ double t_diff = t1 - t0;
+
+ elapsed_dist = sqrt(vel_msg.linear.x*vel_msg.linear.x + vel_msg.linear.y*vel_msg.linear.y)*t_diff;
+ feedback.abs_distance = elapsed_dist;
+ }
+
+ //stop the turtle from moving
+ vel_msg.linear.x = 0;
+ vel_msg.linear.y = 0;
+ pub.publish(vel_msg);
+
+ //Get the stop time
+ double t_end = ros::Time::now().toSec();
+
+ //set the result
+ result.time = (int) (t_end - t0);
+
+ }
+};
+
+int main(int argc, char **argv) {
+
+ ros::init(argc, argv, "moveTurtle");
+ MoveTurtleAction moveTurtle("moveTurtle");
+ ros::spin();
+
+ return 0;
+
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/resetTurtle.cpp b/software_training_assignment/src/resetTurtle.cpp
new file mode 100644
index 0000000..be1840b
--- /dev/null
+++ b/software_training_assignment/src/resetTurtle.cpp
@@ -0,0 +1,44 @@
+/*
+Server for the Reset Turtle Service
+*/
+
+#include
+#include
+//Including the service files for resetTurtle service
+#include
+
+//Defining the service class
+class TurtleReset {
+public:
+ int start_x = 25; //Starting x pos for moving turtle
+ int start_y = 10; //Starting y pos for moving turtle
+ std::string turtle_name = "moving_turtle";
+ TurtleReset() {}
+
+ bool resetTurtlePosition(software_training_assignment::resetTurtleRequest &request,
+ software_training_assignment::resetTurtleResponse &response){
+ ROS_INFO("Turtle Position Reset");
+
+ request.x = start_x;
+ request.y = start_y;
+ request.name = turtle_name;
+ response.success = true;
+
+ return true;
+ };
+}
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "resetTurtle");
+ ros::NodeHandle nh;
+
+ //Create an instance of the class
+ TurtleReset turtle;
+
+ //Link up the service
+ ros::ServiceServer resetTurtle = nh.advertiseService(
+ "resetTurtle", &TurtleReset::resetTurtlePosition, &turtle);
+
+ ros::spin(); //Creates a infinite loop to process callbacks
+ return 0;
+}
\ No newline at end of file
diff --git a/software_training_assignment/srv/resetTurtle.srv b/software_training_assignment/srv/resetTurtle.srv
new file mode 100644
index 0000000..2979f61
--- /dev/null
+++ b/software_training_assignment/srv/resetTurtle.srv
@@ -0,0 +1,5 @@
+int64 x # X Coordinate to reset to
+int64 y # Y Coordinate to reset to
+string name # Name of turtle to be reset
+---
+bool success # Response indicating if it was successful
\ No newline at end of file