diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt
new file mode 100644
index 0000000..4ba2e27
--- /dev/null
+++ b/software_training_assignment/CMakeLists.txt
@@ -0,0 +1,235 @@
+cmake_minimum_required(VERSION 2.8.3)
+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
+ message_generation
+ actionlib_msgs
+ actionlib
+)
+
+## find_package(catkin REQUIRED COMPONENTS actionlib_msgs)
+
+
+## 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
+ PositionData.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+ FILES
+ ResetMovingTurtle.srv
+)
+
+## Generate actions in the 'action' folder
+add_action_files(
+ DIRECTORY action
+ FILES MoveTurtle.action
+)
+
+## Generate added messages and services with any dependencies listed here
+
+
+generate_messages(
+ DEPENDENCIES actionlib_msgs 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 message_runtime actionlib_msgs
+# 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/software_training_assignment_node.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
+# install(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)
+
+add_executable(spawn_turtles src/spawn_turtles.cpp)
+target_link_libraries(spawn_turtles ${catkin_LIBRARIES})
+add_dependencies(spawn_turtles software_training_assignment_generate_messages_cpp)
+
+add_executable(clear_turtle src/clear_turtle.cpp)
+target_link_libraries(clear_turtle ${catkin_LIBRARIES})
+add_dependencies(clear_turtle software_training_assignment_generate_messages_cpp)
+
+add_executable(reset_moving_turtle src/reset_moving_turtle.cpp)
+target_link_libraries(reset_moving_turtle ${catkin_LIBRARIES})
+add_dependencies(reset_moving_turtle software_training_assignment_generate_messages_cpp)
+
+add_executable(position_data_publisher src/position_data_publisher.cpp)
+target_link_libraries(position_data_publisher ${catkin_LIBRARIES})
+add_dependencies(position_data_publisher software_training_assignment_generate_messages_cpp)
+
+add_executable(move_turtle_server src/move_turtle_server.cpp)
+target_link_libraries(move_turtle_server ${catkin_LIBRARIES})
+add_dependencies(move_turtle_server software_training_assignment_generate_messages_cpp)
+
+add_executable(move_turtle_client src/move_turtle_client.cpp)
+target_link_libraries(move_turtle_client ${catkin_LIBRARIES})
+add_dependencies(move_turtle_client software_training_assignment_generate_messages_cpp)
diff --git a/software_training_assignment/MoveTurtle.action b/software_training_assignment/MoveTurtle.action
new file mode 100644
index 0000000..c7b9eef
--- /dev/null
+++ b/software_training_assignment/MoveTurtle.action
@@ -0,0 +1,11 @@
+# Define the goal
+float32 goal_x
+float32 goal_y
+---
+# Define the result
+float32 time_taken
+---
+# Define a feedback message
+float32 distance_to_goal
+
+
diff --git a/software_training_assignment/README.md b/software_training_assignment/README.md
new file mode 100644
index 0000000..2b4b764
--- /dev/null
+++ b/software_training_assignment/README.md
@@ -0,0 +1,14 @@
+Create a ros package called software_training_assignment
+Install the turtlesim package(hint: you shouldn't be downloading any source code from github. You should be using a package manager).
+Create a launch file that launches the turtlesim node with a background colour of (r=128, b=0, g=128)
+Hint: http://wiki.ros.org/Names and http://wiki.ros.org/roslaunch/XML/group are useful. You need to make sure the parameter values are set in the correct ROS namepace (bugged upstream)
+Write a node that:
+Clears any existing turtles
+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 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.
+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 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. To easily test your action, you can use the rqt_action rqt plugin.
+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.
+Create a .gitignore file so that only your source files and CMakelists are committed
+Push your package to a personal github repo and msg Melvin with the link(root directory of the repo should only contain your package folder).
diff --git a/software_training_assignment/ResetMovingTurtle.srv b/software_training_assignment/ResetMovingTurtle.srv
new file mode 100644
index 0000000..410e0f9
--- /dev/null
+++ b/software_training_assignment/ResetMovingTurtle.srv
@@ -0,0 +1,2 @@
+---
+bool success
diff --git a/software_training_assignment/action/MoveTurtle.action b/software_training_assignment/action/MoveTurtle.action
new file mode 100644
index 0000000..fe6c39b
--- /dev/null
+++ b/software_training_assignment/action/MoveTurtle.action
@@ -0,0 +1,6 @@
+float32 goal_x
+float32 goal_y
+---
+float32 time_taken
+---
+float32 distance_to_goal
diff --git a/software_training_assignment/msg/PositionData.msg b/software_training_assignment/msg/PositionData.msg
new file mode 100644
index 0000000..224e98b
--- /dev/null
+++ b/software_training_assignment/msg/PositionData.msg
@@ -0,0 +1,3 @@
+float32 distance_x
+float32 distance_y
+float32 distance
\ No newline at end of file
diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml
new file mode 100644
index 0000000..0558581
--- /dev/null
+++ b/software_training_assignment/package.xml
@@ -0,0 +1,75 @@
+
+
+ software_training_assignment
+ 0.0.0
+ The software_training_assignment package
+
+
+
+
+ tanavya
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ message_generation
+
+
+
+
+
+ message_runtime
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ actionlib
+ actionlib_msgs
+ roscpp
+ rospy
+ std_msgs
+ actionlib
+ actionlib_msgs
+ roscpp
+ rospy
+ std_msgs
+ message_generation
+ actionlib
+ actionlib_msgs
+
+
+
+
+
+
+
+
diff --git a/software_training_assignment/src/clear_turtle.cpp b/software_training_assignment/src/clear_turtle.cpp
new file mode 100644
index 0000000..1540a83
--- /dev/null
+++ b/software_training_assignment/src/clear_turtle.cpp
@@ -0,0 +1,22 @@
+#include "ros/ros.h"
+#include
+#include
+#include
+
+
+int main(int argc, char **argv) {
+
+ ros::init(argc, argv, "clear_turtle");
+
+ ros::NodeHandle nh;
+
+ ros::ServiceClient resetClient = nh.serviceClient("/reset");
+ std_srvs::Empty srv1;
+ resetClient.call(srv1);
+
+ ros::ServiceClient killClient = nh.serviceClient("/kill");
+ turtlesim::Kill srv2;
+ srv2.request.name = "turtle1";
+ killClient.call(srv2);
+
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/move_turtle_client.cpp b/software_training_assignment/src/move_turtle_client.cpp
new file mode 100644
index 0000000..b1f0d61
--- /dev/null
+++ b/software_training_assignment/src/move_turtle_client.cpp
@@ -0,0 +1,42 @@
+#include
+#include
+#include
+#include
+#include
+
+
+int main(int argc, char **argv) {
+
+ ros::init(argc, argv, "test_move_turtle");
+
+ actionlib::SimpleActionClient ac("move_turtle", true);
+ ROS_INFO("Waiting for action server to start.");
+ ac.waitForServer();
+ ROS_INFO("Action server started.");
+ software_training_assignment::MoveTurtleGoal goal;
+ //software_training_assignment::MoveTurtleActionGoal goal; //??
+
+ float goal_x, goal_y;
+
+ std::cout << "Enter x-coordinate: ";
+ std::cin >> goal_x;
+ std::cout << "Enter y-coordinate: ";
+ std::cin >> goal_y;
+
+ goal.goal_x = goal_x;
+ goal.goal_y = goal_y;
+
+ ac.sendGoal(goal);
+
+ bool finished_before_timeout = ac.waitForResult(ros::Duration(300.0));
+
+ if (finished_before_timeout) {
+ actionlib::SimpleClientGoalState state = ac.getState();
+ ROS_INFO("Action finished: %s",state.toString().c_str());
+ }
+ else
+ ROS_INFO("Action did not finish before the time out.");
+
+ return 0;
+
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/move_turtle_server.cpp b/software_training_assignment/src/move_turtle_server.cpp
new file mode 100644
index 0000000..64b6c48
--- /dev/null
+++ b/software_training_assignment/src/move_turtle_server.cpp
@@ -0,0 +1,210 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+
+typedef actionlib::SimpleActionServer Server;
+
+float moving_turtle_x, moving_turtle_y;
+
+void setMovingTurtlePose(const turtlesim::Pose::ConstPtr& msg) {
+ moving_turtle_x = msg->x;
+ moving_turtle_y = msg->y;
+}
+
+
+class MoveTurtleAction {
+
+ protected:
+ ros::NodeHandle nh;
+ Server as;
+ std::string action_name;
+
+ software_training_assignment::MoveTurtleFeedback feedback;
+ software_training_assignment::MoveTurtleResult result;
+
+ //float moving_turtle_x, moving_turtle_y;
+
+ public:
+
+ /*
+ void setMovingTurtlePose(const turtlesim::Pose::ConstPtr& msg) {
+ moving_turtle_x = msg->x;
+ moving_turtle_y = msg->y;
+ }*/
+
+ MoveTurtleAction(string name) : as(nh, name, boost::bind(&MoveTurtleAction::executeCB, this, _1), false), action_name(name) {
+ as.start();
+ }
+
+ ~MoveTurtleAction(void) {}
+
+
+
+
+ void quick_move(const software_training_assignment::MoveTurtleGoalConstPtr &goal) {
+
+ ros::Subscriber pose_sub = nh.subscribe("/moving_turtle/pose", 1000, setMovingTurtlePose);
+ ros::Publisher cmd_vel_pub = nh.advertise("moving_turtle/cmd_vel", 1000);
+
+ ROS_INFO("%s: Executing, moving moving_turtle to (%f %f).", action_name.c_str(), goal->goal_x, goal->goal_y);
+
+ bool success = true;
+
+ ros::spinOnce();
+
+ ROS_INFO("Current x: %f", moving_turtle_x);
+ ROS_INFO("Current y: %f", moving_turtle_y);
+
+ float relative_angle = atan(((goal->goal_y - moving_turtle_y))/(goal->goal_x - moving_turtle_x));
+ float angular_speed = relative_angle;
+ float target_distance = sqrt(pow(moving_turtle_x - goal->goal_x, 2) + pow(moving_turtle_y - goal->goal_y, 2));
+
+ ROS_INFO("Relative Angle : %f", relative_angle);
+ ROS_INFO("Angular Speed : %f", angular_speed);
+ ROS_INFO("Target Distance : %f", target_distance);
+
+ geometry_msgs::Twist msg;
+
+ msg.linear.x = msg.linear.y = msg.linear.z = msg.angular.x = msg.angular.y = 0;
+ msg.angular.z = angular_speed;
+
+ float current_angle = 0;
+ float current_distance = 0;
+
+ ros::Rate r(1);
+
+ ros::Time begin = ros::Time::now();
+ r.sleep();
+ cmd_vel_pub.publish(msg);
+
+
+ msg.angular.z = 0;
+ msg.linear.x = target_distance;
+ r.sleep();
+
+ cmd_vel_pub.publish(msg);
+
+
+ msg.angular.z = -angular_speed;
+ msg.linear.x = 0;
+ r.sleep();
+ cmd_vel_pub.publish(msg);
+
+ r.sleep();
+ ros::spinOnce();
+
+ ROS_INFO("Final x: %f", moving_turtle_x);
+ ROS_INFO("Final y: %f", moving_turtle_y);
+
+ if (success) {
+ result.time_taken = (ros::Time::now() - begin).toSec();
+ ROS_INFO("%s: Succeeded", action_name.c_str());
+ as.setSucceeded(result);
+ }
+
+ }
+
+ void executeCB(const software_training_assignment::MoveTurtleGoalConstPtr &goal) {
+
+ //quick_move(goal);
+ //return;
+
+ ros::Subscriber pose_sub = nh.subscribe("/moving_turtle/pose", 1000, setMovingTurtlePose);
+ ros::Publisher cmd_vel_pub = nh.advertise("moving_turtle/cmd_vel", 1000);
+
+ ROS_INFO("%s: Executing, moving moving_turtle to (%f %f).", action_name.c_str(), goal->goal_x, goal->goal_y);
+
+ bool success = true;
+
+ ros::spinOnce();
+
+ float relative_angle = atan(((goal->goal_y - moving_turtle_y))/(goal->goal_x - moving_turtle_x));
+ float angular_speed = 0.1;
+ float target_distance = sqrt(pow(moving_turtle_x - goal->goal_x, 2) + pow(moving_turtle_y - goal->goal_y, 2));
+ float linear_speed = 0.5;
+ float current_angle = 0;
+ float current_distance = 0;
+ result.time_taken = 0;
+
+ ROS_INFO("Relative Angle : %f", relative_angle);
+ ROS_INFO("Angular Speed : %f", angular_speed);
+ ROS_INFO("Target Distance : %f", target_distance);
+
+ geometry_msgs::Twist msg;
+
+ msg.linear.x = msg.linear.y = msg.linear.z = msg.angular.x = msg.angular.y = 0;
+ msg.angular.z = angular_speed;
+
+
+ ros::Rate r(100);
+
+ ros::Time begin = ros::Time::now();
+ r.sleep();
+ cmd_vel_pub.publish(msg);
+
+
+
+ while (current_angle < relative_angle) {
+ ROS_INFO("Current Angle: %f", current_angle);
+ cmd_vel_pub.publish(msg);
+
+ float change = (ros::Time::now() - begin).toSec();
+ current_angle = angular_speed * change;
+
+ if (as.isPreemptRequested() || !ros::ok()) {
+ ROS_INFO("%s: Preempted", action_name.c_str());
+ as.setPreempted();
+ success = false;
+ break;
+ }
+
+ feedback.distance_to_goal = target_distance;
+ as.publishFeedback(feedback);
+ r.sleep();
+ }
+
+ msg.linear.x = linear_speed;
+ msg.angular.z = 0;
+ result.time_taken = (ros::Time::now() - begin).toSec();
+
+ begin = ros::Time::now();
+
+ while (current_distance < target_distance) {
+ ROS_INFO("Current Distance: %f", current_distance);
+ cmd_vel_pub.publish(msg);
+ float change = (ros::Time::now() - begin).toSec();
+ current_distance = linear_speed * change;
+ feedback.distance_to_goal = target_distance - current_distance;
+ as.publishFeedback(feedback);
+ r.sleep();
+ }
+
+
+
+
+ if (success) {
+ result.time_taken += (ros::Time::now() - begin).toSec();
+ ROS_INFO("%s: Succeeded", action_name.c_str());
+ //set action state to succeeded
+ as.setSucceeded(result);
+ }
+ }
+
+};
+
+int main(int argc, char** argv){
+
+ ros::init(argc, argv, "move_turtle");
+
+ MoveTurtleAction move_turtle("move_turtle");
+
+ ros::spin();
+
+ return 0;
+}
diff --git a/software_training_assignment/src/position_data_publisher.cpp b/software_training_assignment/src/position_data_publisher.cpp
new file mode 100644
index 0000000..59fc57e
--- /dev/null
+++ b/software_training_assignment/src/position_data_publisher.cpp
@@ -0,0 +1,60 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include
+#include
+#include
+#include
+
+float moving_turtle_x, moving_turtle_y, distance_x = -1, distance_y = -1, distance;
+
+void setMovingTurtlePose(const turtlesim::Pose::ConstPtr& msg) {
+ moving_turtle_x = msg->x;
+ moving_turtle_y = msg->y;
+}
+
+void calculateDistance(const turtlesim::Pose::ConstPtr& msg) {
+ distance_x = abs(moving_turtle_x - (msg->x));
+ distance_y = abs(moving_turtle_y - (msg->y));
+ distance = sqrt(pow(distance_x, 2) + pow(distance_y, 2));
+}
+
+
+int main(int argc, char **argv) {
+
+ ros::init(argc, argv, "position_data_publisher");
+ ros::NodeHandle n;
+
+ ros::Publisher position_pub = n.advertise("position_data", 1000);
+
+ ros::Rate loop_rate(10);
+
+
+ //ros::Subscriber sub1 = n.subscribe("/moving_turtle/pose", 1000, setMovingTurtlePose);
+ //ros::Subscriber sub2 = n.subscribe("/stationary_turtle/pose", 1000, calculateDistance);
+
+ //ros::spin();
+
+ ros::Subscriber sub1 = n.subscribe("/moving_turtle/pose", 1000, setMovingTurtlePose);
+ ros::Subscriber sub2 = n.subscribe("/stationary_turtle/pose", 1000, calculateDistance);
+
+
+ while (ros::ok()) {
+
+
+ software_training_assignment::PositionData msg;
+ msg.distance_x = distance_x;
+ msg.distance_y = distance_y;
+ msg.distance = distance;
+
+ ROS_INFO("distance_x: [%f]", msg.distance_x);
+ ROS_INFO("distance_y: [%f]", msg.distance_y);
+ ROS_INFO("distance : [%f]", msg.distance);
+
+ position_pub.publish(msg);
+
+ ros::spinOnce();
+
+ loop_rate.sleep();
+
+ }
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/reset_moving_turtle.cpp b/software_training_assignment/src/reset_moving_turtle.cpp
new file mode 100644
index 0000000..f67a8e9
--- /dev/null
+++ b/software_training_assignment/src/reset_moving_turtle.cpp
@@ -0,0 +1,29 @@
+#include "ros/ros.h"
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+
+bool reset_turtle(software_training_assignment::ResetMovingTurtle::Request &req, software_training_assignment::ResetMovingTurtle::Response &res) {
+ ros::NodeHandle nh;
+ ros::ServiceClient resetTurtle = nh.serviceClient("/moving_turtle/teleport_absolute");
+ turtlesim::TeleportAbsolute srv;
+ srv.request.x = 0;
+ srv.request.y = 0;
+ srv.request.theta = 0;
+ res.success = resetTurtle.call(srv);
+ return true;
+}
+
+int main(int argc, char **argv) {
+
+ ros::init(argc, argv, "reset_moving_turtle");
+ ros::NodeHandle n;
+ ros::ServiceServer service = n.advertiseService("reset_moving_turtle", reset_turtle);
+ ROS_INFO("Ready to reset_moving_turtle.");
+ ros::spin();
+
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/spawn_turtles.cpp b/software_training_assignment/src/spawn_turtles.cpp
new file mode 100644
index 0000000..802f52a
--- /dev/null
+++ b/software_training_assignment/src/spawn_turtles.cpp
@@ -0,0 +1,30 @@
+#include "ros/ros.h"
+#include
+#include
+#include
+
+using namespace std;
+
+void spawnTurtle(float x, float y, float theta, string name) {
+
+ ros::NodeHandle nh;
+ ros::ServiceClient spawnTurtle = nh.serviceClient("/spawn");
+ turtlesim::Spawn srv;
+ srv.request.x = x;
+ srv.request.y = y;
+ srv.request.theta = theta;
+ srv.request.name = name;
+ spawnTurtle.call(srv);
+}
+
+
+int main(int argc, char **argv) {
+
+ ros::init(argc, argv, "spawn_turtles");
+
+ ros::NodeHandle nh;
+
+ spawnTurtle(5, 5, 0, "stationary_turtle");
+ spawnTurtle(25, 10, 0, "moving_turtle");
+
+}
\ No newline at end of file
diff --git a/software_training_assignment/srv/ResetMovingTurtle.srv b/software_training_assignment/srv/ResetMovingTurtle.srv
new file mode 100644
index 0000000..410e0f9
--- /dev/null
+++ b/software_training_assignment/srv/ResetMovingTurtle.srv
@@ -0,0 +1,2 @@
+---
+bool success