diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0517ffe
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+#nothing here
diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt
new file mode 100644
index 0000000..d837611
--- /dev/null
+++ b/software_training/CMakeLists.txt
@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(software_training)
+
+## 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
+ std_msgs
+ message_generation
+ actionlib_msgs
+ actionlib
+)
+
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+ FILES
+ software_training_message.msg
+
+ )
+
+## Generate services in the 'srv' folder
+ add_service_files(
+ FILES
+ reset_service.srv
+
+ )
+
+## Generate actions in the 'action' folder
+ add_action_files(
+ FILES
+ software_training_action.action
+
+ )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+ DEPENDENCIES
+ std_msgs
+ actionlib_msgs
+ #reset_service
+ )
+
+
+
+
+catkin_package(
+ CATKIN_DEPENDS
+ roscpp
+ std_msgs
+ message_runtime
+ actionlib_msgs
+
+)
+
+
+add_executable(software_training_node src/software_training_node.cpp)
+target_link_libraries(software_training_node ${catkin_LIBRARIES})
+add_dependencies(software_training_node software_training_generate_messages_cpp)
+
+add_executable(moving_turtle_service src/moving_turtle_service.cpp)
+target_link_libraries(moving_turtle_service ${catkin_LIBRARIES})
+add_dependencies(moving_turtle_service software_training_generate_messages_cpp)
+
+add_executable(moving_turtle_action_node src/moving_turtle_action_node.cpp)
+target_link_libraries(moving_turtle_action_node ${catkin_LIBRARIES})
+add_dependencies(moving_turtle_action_node software_training_generate_messages_cpp)
\ No newline at end of file
diff --git a/software_training/action/software_training_action.action b/software_training/action/software_training_action.action
new file mode 100644
index 0000000..3b5aebb
--- /dev/null
+++ b/software_training/action/software_training_action.action
@@ -0,0 +1,10 @@
+#goal
+float32 x_position_to_waypoint
+float32 y_position_to_waypoint
+---
+#result
+duration total_time
+---
+#feedback
+float32 x_distance_to_goal
+float32 y_distance_to_goal
\ No newline at end of file
diff --git a/software_training/launch/software_training.launch b/software_training/launch/software_training.launch
new file mode 100644
index 0000000..7e685b5
--- /dev/null
+++ b/software_training/launch/software_training.launch
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/software_training/msg/software_training_message.msg b/software_training/msg/software_training_message.msg
new file mode 100644
index 0000000..6b6d8bd
--- /dev/null
+++ b/software_training/msg/software_training_message.msg
@@ -0,0 +1,3 @@
+float64 x_stationary_turtle_to_moving_turtle
+float64 y_stationary_turtle_to_moving_turtle
+float64 distance
\ No newline at end of file
diff --git a/software_training/package.xml b/software_training/package.xml
new file mode 100644
index 0000000..9793199
--- /dev/null
+++ b/software_training/package.xml
@@ -0,0 +1,68 @@
+
+
+ software_training
+ 0.0.0
+ The software_training package
+
+
+
+
+ niiquaye
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ std_msgs
+ message_generation
+ actionlib_msgs
+ roscpp
+ std_msgs
+ roscpp
+ std_msgs
+ message_runtime
+ actionlib_msgs
+
+
+
+
+
+
+
diff --git a/software_training/src/moving_turtle_action_node.cpp b/software_training/src/moving_turtle_action_node.cpp
new file mode 100644
index 0000000..97df5d0
--- /dev/null
+++ b/software_training/src/moving_turtle_action_node.cpp
@@ -0,0 +1,103 @@
+#include "ros/ros.h"
+#include "actionlib/server/simple_action_server.h"
+#include "software_training/software_training_actionAction.h"
+#include "geometry_msgs/Twist.h"
+#include
+
+class moving_turtle_action_server{
+
+ protected:
+ //create node handle
+ ros::NodeHandle nh;
+ //create action server
+ actionlib::SimpleActionServer
+ action_server;
+ //action server name
+ std::string name;
+ //create feedback for action server
+ software_training::software_training_actionFeedback feedback;
+ //create result for action server
+ software_training::software_training_actionResult result;
+ //create publisher to cmd_vel topic for moving turtle
+ ros::Publisher pub_moving_turtle;
+ public:
+ //constructor
+ moving_turtle_action_server(std::string _name_of_server);
+ //callback function
+ void executeCB(const software_training::software_training_actionGoalConstPtr& goal);
+
+};
+
+moving_turtle_action_server::moving_turtle_action_server(std::string _name_of_server):
+ action_server(nh,_name_of_server, boost::bind(&moving_turtle_action_server::executeCB, this, _1), false),
+ name(_name_of_server)
+{
+ //start action server
+ action_server.start();
+ // instantiate publisher
+ pub_moving_turtle = nh.advertise("/moving_turtle/cmd_vel",1000);
+}
+
+void moving_turtle_action_server::executeCB(
+ const software_training::software_training_actionGoalConstPtr& goal)
+{
+ //set a variable to check if finished
+ //set ros rate
+ ros::Rate loop_rate{1};
+ //get components of the goal
+ double x_goal = goal->x_position_to_waypoint;
+ double y_goal = goal->y_position_to_waypoint;
+ //create a message to publish
+ geometry_msgs::Twist msg;
+ //initialize message to be published
+ msg.linear.x = 0.0;
+ msg.linear.y = 0.0;
+ msg.linear.z = 0.0;
+ msg.angular.x = 0.0;
+ msg.angular.y = 0.0;
+ msg.angular.z = 0.0;
+
+ //start of timer
+ ros::Time start = ros::Time::now();
+
+ while(ros::ok() && action_server.isActive() && !action_server.isNewGoalAvailable()){
+ //publish to cmd_vel topic one interval at a time
+ if(msg.linear.x < x_goal || msg.linear.y < y_goal){
+ if(msg.linear.x < x_goal){
+ msg.linear.x++;
+ feedback.x_distance_to_goal = x_goal - msg.linear.x;
+ }
+ if(msg.linear.y < y_goal){
+ msg.linear.y++;
+ feedback.y_distance_to_goal = y_goal - msg.linear.y;
+ }
+ //publish message
+ pub_moving_turtle.publish(msg);
+ //publish feedback
+ action_server.publishFeedback(feedback);
+ }
+ else if(msg.linear.x >= x_goal && msg.linear.y >= y_goal) break;
+ loop_rate.sleep();
+ }
+ //check to see if action is still up and running since we broke the loop
+ if(!ros::ok() || action_server.isPreemptRequested() || !action_server.isActive()){
+ ROS_ERROR_STREAM("moving_turtle_action_server NOT AVAILABLE/SHUTDOWN/NEW GOAL");
+ }
+ else{
+ //end of timer
+ ros::Time end = ros::Time::now();
+ //get total time
+ ros::Duration res_time = end - start;
+ result.total_time = res_time;
+ //set result
+ action_server.setSucceeded(result);
+ }
+
+}
+
+int main(int argc, char **argv){
+ ros::init(argc, argv, "moving_turtle_action_node");
+ moving_turtle_action_server as("mover");
+ ros::spin();
+ return 0;
+}
\ No newline at end of file
diff --git a/software_training/src/moving_turtle_service.cpp b/software_training/src/moving_turtle_service.cpp
new file mode 100644
index 0000000..239b3fd
--- /dev/null
+++ b/software_training/src/moving_turtle_service.cpp
@@ -0,0 +1,49 @@
+#include "ros/ros.h"
+#include "software_training/reset_service.h"
+#include "turtlesim/TeleportAbsolute.h"
+
+class server{
+ public:
+ //callback function for server
+ bool callback(software_training::reset_service::Request& request,
+ software_training::reset_service::Response& response);
+ // helper function to call moving_turtle_client - client call
+ bool moving_turtle_client();
+
+};
+
+bool server::callback(software_training::reset_service::Request& request,
+ software_training::reset_service::Response& response){
+ response.success = (moving_turtle_client())? true : false;
+ return true;
+}
+
+bool server::moving_turtle_client(){
+ turtlesim::TeleportAbsolute srv;
+ srv.request.x = 5.544444561004639;
+ srv.request.y = 5.544444561004639;
+ srv.request.theta = 0;
+ ros::NodeHandle nh;
+ ros::ServiceClient client = nh.serviceClient
+ ("/moving_turtle/teleport_absolute");
+ if(client.call(srv)){
+ srv.response;
+ ROS_INFO_STREAM("'moving_turtle' reseted");
+ return true;
+ }
+ else{
+ ROS_ERROR_STREAM("the moving turtle service failed to call");
+ return false;
+ }
+
+}
+
+
+int main(int argc, char ** argv){
+ ros::init(argc, argv, "moving_turtle_reset_server_node");
+ ros::NodeHandle nh;
+ server srv_object;
+ ros::ServiceServer srv = nh.advertiseService("moving_turtle_service", &server::callback, &srv_object);
+ ros::spin();
+ return 0;
+}
\ No newline at end of file
diff --git a/software_training/src/software_training_node.cpp b/software_training/src/software_training_node.cpp
new file mode 100755
index 0000000..6efbcd2
--- /dev/null
+++ b/software_training/src/software_training_node.cpp
@@ -0,0 +1,147 @@
+#include "ros/ros.h"
+#include "turtlesim/Spawn.h"
+#include "std_msgs/String.h"
+#include "std_srvs/Empty.h"
+#include "software_training/software_training_message.h"
+#include "turtlesim/Pose.h"
+#include "math.h"
+
+class software_training_class{
+
+ private:
+ //clients
+ ros::ServiceClient clientReset;
+ ros::ServiceClient clientSpawn1;
+ ros::ServiceClient clientMovingTurt;
+ //node handles
+ ros::NodeHandle nh;
+ //subscribers
+ ros::Subscriber moving_turtle_pose_sub;
+ ros::Subscriber stationary_turtle_pose_sub;
+ //publisher
+ ros::Publisher pub;
+ //publisher rate
+ ros::Rate loop_rate{10};
+ //turtle coordinates
+ float x_moving_turtle_distance;
+ float y_moving_turtle_distance;
+ float x_stationary_turtle_distance;
+ float y_stationary_turtle_distance;
+
+ public:
+ //constructor
+ software_training_class();
+ //client that creates a turtle
+ void clientSpawnStationaryTurtle();
+ //client that resets everything
+ void clientResetTurtle();
+ //client that creates moving turtle
+ void clientSpawnMovingTurtle();
+ //subscriber call back for moving turtle position
+ void movingTurtCallback(const turtlesim::Pose::ConstPtr& msg);
+ //subscriber call back for moving turtle position
+ void stationaryTurtCallback(const turtlesim::Pose::ConstPtr& msg);
+ //run publisher
+ void runPublisher();
+
+};
+
+software_training_class::software_training_class(){
+ clientResetTurtle();
+ clientSpawnStationaryTurtle();
+ clientSpawnMovingTurtle();
+ //subscribers
+ moving_turtle_pose_sub = nh.subscribe("/moving_turtle/pose", 100, &software_training_class::movingTurtCallback, this);
+ stationary_turtle_pose_sub = nh.subscribe("/stationary_turtle/pose", 100, &software_training_class::stationaryTurtCallback, this);
+}
+
+void software_training_class::clientSpawnStationaryTurtle(){
+ turtlesim::Spawn srv;
+ srv.request.x = 5;
+ srv.request.y = 5;
+ srv.request.theta = 0.0;
+ srv.request.name = "stationary_turtle";
+ clientSpawn1 = nh.serviceClient("/spawn");
+ //wait for server to exist
+ clientSpawn1.waitForExistence(ros::Duration(-1));
+ if(clientSpawn1.call(srv)){
+ std_msgs::String ss;
+ ss.data = srv.response.name;
+ ROS_INFO("The name of the turtle %s has been spawned", ss.data.c_str());
+ }
+ else{
+ ROS_ERROR("Failed to call '/spawn' service");
+ }
+}
+
+void software_training_class::clientResetTurtle(){
+ std_srvs::Empty srv;
+ // no arguments
+ srv.request;
+ clientReset = nh.serviceClient("/clear");
+ //wait for server to exit
+ clientReset.waitForExistence(ros::Duration(-1));
+ if(clientReset.call(srv)){
+ srv.response;
+ ROS_INFO_STREAM("All turtles have been cleared");
+ }
+ else{
+ ROS_ERROR_STREAM("Failed to call '/reset' service");
+ }
+}
+
+void software_training_class::clientSpawnMovingTurtle(){
+ turtlesim::Spawn srv;
+ srv.request.x = 25;
+ srv.request.y = 10;
+ srv.request.theta = 0;
+ srv.request.name = "moving_turtle";
+ clientMovingTurt = nh.serviceClient("/spawn");
+ //wait for server existence
+ clientMovingTurt.waitForExistence(ros::Duration(-1));
+ if(clientMovingTurt.call(srv)){
+ std_msgs::String ss;
+ ss.data = srv.response.name;
+ ROS_INFO("The turtle %s has been spawned at x=%f and y=%f", ss.data.c_str(), srv.request.x, srv.request.y);
+ }
+ else{
+ ROS_ERROR("the turtle %s has NOT been spawned", srv.request.name.c_str());
+ }
+}
+
+void software_training_class::movingTurtCallback(const turtlesim::Pose::ConstPtr& msg){
+ x_moving_turtle_distance = msg->x;
+ y_moving_turtle_distance = msg->y;
+
+}
+
+void software_training_class::stationaryTurtCallback(const turtlesim::Pose::ConstPtr& msg){
+ x_stationary_turtle_distance = msg->x;
+ y_stationary_turtle_distance = msg->y;
+
+}
+
+void software_training_class::runPublisher(){
+ pub = nh.advertise("moving_turtle_stationary_turtle_distance",100);
+ while(ros::ok()){
+ //create message to be published
+ software_training::software_training_message msg;
+ float abs_x_diff{abs(x_stationary_turtle_distance - x_moving_turtle_distance)};
+ float abs_y_diff{abs(y_moving_turtle_distance - y_stationary_turtle_distance)};
+ //fill message contents
+ msg.x_stationary_turtle_to_moving_turtle = abs_x_diff;
+ msg.y_stationary_turtle_to_moving_turtle = abs_y_diff;
+ msg.distance = sqrt((abs_x_diff*abs_x_diff) + (abs_y_diff*abs_y_diff));
+ //actually publish message
+ pub.publish(msg);
+ //allow for subs to access callbacks
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+}
+int main(int argc, char ** argv){
+ ros::init(argc, argv, "software_training_node");
+ software_training_class node;
+ node.runPublisher();
+ return 0;
+}
diff --git a/software_training/srv/reset_service.srv b/software_training/srv/reset_service.srv
new file mode 100644
index 0000000..e2c59ee
--- /dev/null
+++ b/software_training/srv/reset_service.srv
@@ -0,0 +1,3 @@
+
+---
+bool success
\ No newline at end of file