diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..be3559b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+dbus-1/
\ No newline at end of file
diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt
new file mode 100644
index 0000000..3ec9442
--- /dev/null
+++ b/software_training_assignment/CMakeLists.txt
@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(software_training_assignment)
+
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ std_msgs
+ message_generation
+ turtlesim
+ actionlib
+ actionlib_msgs
+)
+
+find_package(Boost REQUIRED COMPONENTS system)
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+ FILES
+ Position.msg
+ )
+
+## Generate actions in the 'action' folder
+ add_action_files(
+ FILES
+ Goto.action
+ )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+ DEPENDENCIES
+ std_msgs
+ actionlib_msgs
+ )
+
+catkin_package(
+ CATKIN_DEPENDS message_runtime actionlib_msgs
+)
+
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+ ${Boost_INCLUDE_DIRS}
+)
+
+ install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h"
+ PATTERN ".svn" EXCLUDE
+ )
+
+
+
+
+add_executable(spawnturtle src/spawnturtle.cpp)
+target_link_libraries(spawnturtle ${catkin_LIBRARIES})
+# add_dependencies(spawnturtle software_training_assignment_gencpp)
+
+add_executable(testing src/testing.cpp)
+target_link_libraries(testing ${catkin_LIBRARIES})
diff --git a/software_training_assignment/action/Goto.action b/software_training_assignment/action/Goto.action
new file mode 100644
index 0000000..1d6804f
--- /dev/null
+++ b/software_training_assignment/action/Goto.action
@@ -0,0 +1,6 @@
+float32 x
+float32 y
+---
+duration time_elapsed
+---
+float32 distance
diff --git a/software_training_assignment/launch/turtlebackground.launch b/software_training_assignment/launch/turtlebackground.launch
new file mode 100644
index 0000000..cd24631
--- /dev/null
+++ b/software_training_assignment/launch/turtlebackground.launch
@@ -0,0 +1,10 @@
+
+
+
+ 128
+ 128
+ 0
+
+
+
+
diff --git a/software_training_assignment/msg/Position.msg b/software_training_assignment/msg/Position.msg
new file mode 100644
index 0000000..e5ddcc8
--- /dev/null
+++ b/software_training_assignment/msg/Position.msg
@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 distance
diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml
new file mode 100644
index 0000000..284d5f2
--- /dev/null
+++ b/software_training_assignment/package.xml
@@ -0,0 +1,32 @@
+
+
+ software_training_assignment
+ 0.0.0
+ The software_training_assignment package
+
+ htieu
+
+ MIT
+
+
+ message_generation
+
+ message_runtime
+ gtest
+ catkin
+ roscpp
+ std_msgs
+ roscpp
+ std_msgs
+ roscpp
+ std_msgs
+ turtlesim
+ turtlesim
+ actionlib
+ actionlib
+ actionlib_msgs
+ actionlib_msgs
+
+
+
+
diff --git a/software_training_assignment/src/goto_action.h b/software_training_assignment/src/goto_action.h
new file mode 100644
index 0000000..d8768a0
--- /dev/null
+++ b/software_training_assignment/src/goto_action.h
@@ -0,0 +1,97 @@
+#include
+#include
+#include
+#include
+
+namespace A
+{
+ class GotoAction
+ {
+ public:
+ ros::NodeHandle node;
+ GotoAction(std::string name) : as_(node, name, boost::bind(&GotoAction::executeCB, this, _1), false)
+ {
+ as_.start();
+ ROS_INFO("Action server started");
+ }
+
+ ~GotoAction(void)
+ {
+ }
+
+ ros::Publisher pub;
+ ros::Subscriber sub;
+ double x, y;
+
+ void subCallback(const turtlesim::Pose::ConstPtr& msg)
+ {
+ x = msg->x;
+ y = msg->y;
+ }
+
+ void executeCB(const software_training_assignment::GotoGoal::ConstPtr &goal)
+ {
+
+ ROS_INFO("executeCB called");
+ double relativeX = goal->x - x,
+ relativeY = goal->y - y,
+ relativeDist = sqrt(pow(relativeX, 2) + pow(relativeY, 2)),
+ theta = acos(relativeX / relativeDist);
+
+ if (relativeY < 0)
+ theta *= -1;
+
+ bool success = true;
+
+ geometry_msgs::Twist rotation;
+
+ feedback_.distance = relativeDist;
+ as_.publishFeedback(feedback_);
+
+ rotation.angular.z = theta;
+
+ pub.publish(rotation);
+
+ ros::Duration(1).sleep();
+
+ double gotoCount = (int)relativeDist / 2, gotoRe = relativeDist - (gotoCount * 2);
+ ros::Duration gotoDuration(1);
+
+ geometry_msgs::Twist forward;
+
+ forward.linear.x = 2;
+ for (int count = 0; count < gotoCount; count++)
+ {
+ if(as_.isPreemptRequested() || !ros::ok())
+ {
+ as_.setPreempted();
+ success = false;
+ break;
+ }
+ pub.publish(forward);
+ ros::Duration(1).sleep();
+ gotoDuration += ros::Duration(1);
+
+ }
+
+ forward.linear.x = gotoRe;
+
+ gotoDuration += ros::Duration(2);
+
+ pub.publish(forward);
+ ros::Duration(1).sleep();
+
+ if (success)
+ {
+ result_.time_elapsed = gotoDuration;
+ as_.setSucceeded(result_);
+ }
+
+ }
+
+ protected:
+ software_training_assignment::GotoFeedback feedback_;
+ actionlib::SimpleActionServer as_;
+ software_training_assignment::GotoResult result_;
+ };
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/killTurtles.cpp b/software_training_assignment/src/killTurtles.cpp
new file mode 100644
index 0000000..6d96556
--- /dev/null
+++ b/software_training_assignment/src/killTurtles.cpp
@@ -0,0 +1,26 @@
+#include
+#include "killTurtles.h"
+
+bool killTurtles(ros::NodeHandle n)
+{
+ ros::ServiceClient killClient = n.serviceClient("kill");
+
+ ros::service::waitForService("kill");
+
+ turtlesim::Kill killSrv;
+
+ killSrv.request.name = "turtle1";
+
+
+ if (killClient.call(killSrv))
+ {
+ ROS_INFO_STREAM (killSrv.request.name << " killed");
+ }
+ else
+ {
+ ROS_ERROR("Failed to call service kill");
+ return false;
+ }
+
+ return true;
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/killTurtles.h b/software_training_assignment/src/killTurtles.h
new file mode 100644
index 0000000..60d5bc3
--- /dev/null
+++ b/software_training_assignment/src/killTurtles.h
@@ -0,0 +1,8 @@
+#ifndef KILLTURTLES_H
+#define KILLTURTLES_H
+
+#include
+
+bool killTurtles(ros::NodeHandle n);
+
+#endif
\ No newline at end of file
diff --git a/software_training_assignment/src/publisher.h b/software_training_assignment/src/publisher.h
new file mode 100644
index 0000000..3235fee
--- /dev/null
+++ b/software_training_assignment/src/publisher.h
@@ -0,0 +1,29 @@
+#include
+#include
+#include
+#include
+
+namespace P
+{
+ class Publisher
+ {
+ public:
+ void publisher()
+ {
+ software_training_assignment::Position msg;
+ msg.x = x - 5;
+ msg.y = y - 5;
+ msg.distance = sqrt(pow(msg.x, 2) + pow(msg.y, 2));
+
+ pub.publish(msg);
+ }
+ void subCallback(const turtlesim::Pose::ConstPtr& msg)
+ {
+ x = msg->x;
+ y = msg->y;
+ }
+ double x = 0, y = 0;
+ ros::Publisher pub;
+ ros::Subscriber sub;
+ };
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/resetPos.cpp b/software_training_assignment/src/resetPos.cpp
new file mode 100644
index 0000000..9b94d5a
--- /dev/null
+++ b/software_training_assignment/src/resetPos.cpp
@@ -0,0 +1,31 @@
+#include
+#include "resetPos.h"
+#include "killTurtles.h"
+#include "spawnTurtles.h"
+
+bool resetPos(std_srvs::Empty::Request &req,
+ std_srvs::Empty::Response &res)
+{
+ ros::NodeHandle n;
+
+ ros::ServiceClient resetClient = n.serviceClient("reset");
+
+ ros::service::waitForService("reset");
+
+ std_srvs::Empty resetSrv;
+
+ if (resetClient.call(resetSrv))
+ {
+ killTurtles(n);
+ spawnTurtles(n, "stationary_turtle", 5, 5);
+ spawnTurtles(n, "moving_turtle", 25, 10);
+ ROS_INFO_STREAM ("Resetted");
+ }
+ else
+ {
+ ROS_ERROR("Failed to call service reset");
+ return false;
+ }
+
+ return true;
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/resetPos.h b/software_training_assignment/src/resetPos.h
new file mode 100644
index 0000000..56f409a
--- /dev/null
+++ b/software_training_assignment/src/resetPos.h
@@ -0,0 +1,9 @@
+#ifndef RESETPOS_H
+#define RESETPOS_H
+
+#include
+
+bool resetPos(std_srvs::Empty::Request &req,
+ std_srvs::Empty::Response &res);
+
+#endif
\ No newline at end of file
diff --git a/software_training_assignment/src/spawnTurtles.cpp b/software_training_assignment/src/spawnTurtles.cpp
new file mode 100644
index 0000000..c216c6c
--- /dev/null
+++ b/software_training_assignment/src/spawnTurtles.cpp
@@ -0,0 +1,31 @@
+#include
+#include "spawnTurtles.h"
+
+
+bool spawnTurtles(ros::NodeHandle n, std::string name, int x, int y)
+{
+ ros::ServiceClient spawnClient = n.serviceClient("spawn");
+
+
+ ros::service::waitForService("spawn");
+
+ turtlesim::Spawn spawnSrv;
+
+ spawnSrv.request.x = x;
+ spawnSrv.request.y = y;
+ spawnSrv.request.theta = 0;
+ spawnSrv.request.name = name;
+
+
+ if (spawnClient.call(spawnSrv))
+ {
+ ROS_INFO_STREAM ("Name: " << spawnSrv.response.name);
+ }
+ else
+ {
+ ROS_ERROR("Failed to call service spawn");
+ return false;
+ }
+
+ return true;
+}
\ No newline at end of file
diff --git a/software_training_assignment/src/spawnTurtles.h b/software_training_assignment/src/spawnTurtles.h
new file mode 100644
index 0000000..903253c
--- /dev/null
+++ b/software_training_assignment/src/spawnTurtles.h
@@ -0,0 +1,8 @@
+#ifndef SPAWNTURTLES_H
+#define SPAWNTURTLES_H
+
+#include
+
+bool spawnTurtles(ros::NodeHandle n, std::string name, int x, int y);
+
+#endif
\ No newline at end of file
diff --git a/software_training_assignment/src/spawnturtle.cpp b/software_training_assignment/src/spawnturtle.cpp
new file mode 100644
index 0000000..b5556c0
--- /dev/null
+++ b/software_training_assignment/src/spawnturtle.cpp
@@ -0,0 +1,49 @@
+#include
+#include
+#include
+#include
+#include "killTurtles.cpp"
+#include "publisher.h"
+#include "resetPos.cpp"
+#include "spawnTurtles.cpp"
+#include "goto_action.h"
+
+
+using namespace P;
+using namespace A;
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "spawnturtle");
+
+ ros::NodeHandle n;
+
+ killTurtles(n);
+ spawnTurtles(n, "stationary_turtle", 5, 5);
+ spawnTurtles(n, "moving_turtle", 25, 10);
+
+ ros::ServiceServer resetService = n.advertiseService("reset_pos", resetPos);
+
+ Publisher posPublisher;
+ posPublisher.pub = n.advertise("position", 1000);
+ posPublisher.sub = n.subscribe("moving_turtle/pose", 1000, &Publisher::subCallback, &posPublisher);
+
+ GotoAction goTo("goto");
+ goTo.node = n;
+ goTo.pub = n.advertise("moving_turtle/cmd_vel", 1000);
+ goTo.sub = n.subscribe("moving_turtle/pose", 1000, &GotoAction::subCallback, &goTo);
+
+ ros::Rate loop_rate(10);
+
+ while(ros::ok())
+ {
+ posPublisher.publisher();
+
+ ros::spinOnce();
+
+ loop_rate.sleep();
+ }
+
+ return 0;
+}
diff --git a/software_training_assignment/src/testing.cpp b/software_training_assignment/src/testing.cpp
new file mode 100644
index 0000000..bda2165
--- /dev/null
+++ b/software_training_assignment/src/testing.cpp
@@ -0,0 +1,63 @@
+#include
+#include
+#include
+#include
+#include
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "test_node");
+
+ if (argc != 3)
+ {
+ ROS_INFO("rosrun software_training_assignment testing x y");
+ return 0;
+ }
+
+ ros::NodeHandle n;
+
+ actionlib::SimpleActionClient ac ("turtlesim1/goto", true);
+
+ ROS_INFO("Waiting for action server to start.");
+
+ ac.waitForServer();
+
+ ROS_INFO("Action server started, sending goal.");
+
+
+ software_training_assignment::GotoGoal goal;
+
+ goal.x = atoll(argv[1]);
+ goal.y = atoll(argv[2]);
+
+ ac.sendGoal(goal);
+
+ bool finished_before_timeout = ac.waitForResult(ros::Duration(30.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.");
+
+
+ ros::ServiceClient client = n.serviceClient("turtlesim1/reset_pos");
+
+ ros::service::waitForService("turtlesim1/reset_pos");
+
+ std_srvs::Empty srv;
+
+ if(client.call(srv))
+ {
+ ROS_INFO("Test resetted");
+ }
+ else
+ {
+ ROS_ERROR("Failed to call service reset_pos");
+ return 1;
+ }
+
+ return 0;
+}
\ No newline at end of file