diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt
new file mode 100644
index 0000000..0c21332
--- /dev/null
+++ b/software_training_assignment/CMakeLists.txt
@@ -0,0 +1,213 @@
+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
+ std_msgs
+ turtlesim
+ message_generation
+ actionlib
+ 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
+ Distance.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+ FILES
+ Reset.srv
+)
+
+## Generate actions in the 'action' folder
+add_action_files(
+ FILES
+ Waypoint.action
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+ actionlib_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 std_msgs 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
+
+
+## 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
+
+add_executable(turtle_assignment src/turtle_assignment.cpp)
+target_link_libraries(turtle_assignment ${catkin_LIBRARIES})
+add_dependencies(turtle_assignment software_training_assignment_gencpp)
+
+add_executable(reset_test src/reset_test.cpp)
+target_link_libraries(reset_test ${catkin_LIBRARIES})
+add_dependencies(reset_test software_training_assignment_gencpp)
+
+#############
+## 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)
diff --git a/software_training_assignment/action/Waypoint.action b/software_training_assignment/action/Waypoint.action
new file mode 100644
index 0000000..509f6bf
--- /dev/null
+++ b/software_training_assignment/action/Waypoint.action
@@ -0,0 +1,9 @@
+#goal definition
+float32 x
+float32 y
+---
+#result definition
+float32 distance
+---
+#feedback
+float32 timeElapsed
diff --git a/software_training_assignment/config/background_param.yaml b/software_training_assignment/config/background_param.yaml
new file mode 100644
index 0000000..3544828
--- /dev/null
+++ b/software_training_assignment/config/background_param.yaml
@@ -0,0 +1 @@
+{background_b: 1, background_g: 2, background_r: 3}
diff --git a/software_training_assignment/launch/background.launch b/software_training_assignment/launch/background.launch
new file mode 100644
index 0000000..23cbfca
--- /dev/null
+++ b/software_training_assignment/launch/background.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
diff --git a/software_training_assignment/msg/Distance.msg b/software_training_assignment/msg/Distance.msg
new file mode 100644
index 0000000..c98cf8e
--- /dev/null
+++ b/software_training_assignment/msg/Distance.msg
@@ -0,0 +1,2 @@
+float32 x_dist
+float32 y_dist
diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml
new file mode 100644
index 0000000..c814732
--- /dev/null
+++ b/software_training_assignment/package.xml
@@ -0,0 +1,72 @@
+
+
+ software_training_assignment
+ 0.0.0
+ The software_training_assignment package
+
+
+
+
+ steven
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ std_msgs
+ turtlesim
+ message_generation
+ actionlib
+ actionlib_msgs
+ roscpp
+ std_msgs
+ roscpp
+ std_msgs
+ turtlesim
+ message_runtime
+ actionlib
+ actionlib_msgs
+
+
+
+
+
+
+
diff --git a/software_training_assignment/src/reset_test.cpp b/software_training_assignment/src/reset_test.cpp
new file mode 100644
index 0000000..2aa7c9e
--- /dev/null
+++ b/software_training_assignment/src/reset_test.cpp
@@ -0,0 +1,23 @@
+#include "ros/ros.h"
+#include "software_training_assignment/Reset.h"
+#include
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "reset_test");
+ ros::NodeHandle n;
+ ros::ServiceClient client = n.serviceClient("reset_moving_turtle");
+ software_training_assignment::Reset srv;
+
+ if (client.call(srv))
+ {
+ ROS_INFO("reset_moving_turtle service called. Result: %d", srv.response.success);
+ }
+ else
+ {
+ ROS_ERROR("Failed to call service reset_moving_turtle");
+ return 1;
+ }
+
+ return 0;
+}
diff --git a/software_training_assignment/src/turtle_assignment.cpp b/software_training_assignment/src/turtle_assignment.cpp
new file mode 100644
index 0000000..bd69c48
--- /dev/null
+++ b/software_training_assignment/src/turtle_assignment.cpp
@@ -0,0 +1,90 @@
+#include
+#include "turtlesim/Spawn.h"
+#include "turtlesim/Kill.h"
+#include "turtlesim/TeleportAbsolute.h"
+#include "turtlesim/Pose.h"
+#include "software_training_assignment/Reset.h"
+#include "software_training_assignment/Distance.h"
+//#include "waypoint_action.h"
+
+// Globals
+software_training_assignment::Distance dist_msg;
+
+bool reset(software_training_assignment::Reset::Request&, software_training_assignment::Reset::Response &res){
+ ros::NodeHandle nh;
+ ros::ServiceClient client = nh.serviceClient("moving_turtle/teleport_absolute");
+ turtlesim::TeleportAbsolute tel_srv;
+ tel_srv.request.x = 25.0;
+ tel_srv.request.y = 5.0;
+ tel_srv.request.theta = 0.0;
+
+ if(client.call(tel_srv)){
+ ROS_INFO("moving_turtle moved back to x: 25.0, y: 10.0 theta: 0.0");
+ res.success = true;
+ return true;
+ }
+ ROS_INFO("Failed to reset moving_turtle");
+ res.success = false;
+ return false;
+}
+
+void poseCallback(const turtlesim::Pose::ConstPtr& msg){
+ dist_msg.x_dist = msg->x - 5.0; // Calculate x distance from stationary_turtle
+ dist_msg.y_dist = msg->y - 5.0; // Calculate y distance from stationary_turtle
+ ROS_INFO("x: %f y: %f", dist_msg.x_dist, dist_msg.y_dist);
+}
+
+int main(int argc, char** argv){
+ ros::init(argc, argv, "turtle_assignment");
+ ros::NodeHandle n;
+
+ ros::ServiceClient client = n.serviceClient("kill");
+ turtlesim::Kill kill_srv;
+ kill_srv.request.name = "turtle1";
+ if(client.call(kill_srv)){
+ ROS_INFO("Cleared turtle1");
+ }
+ else{
+ ROS_ERROR("Failed to call service turtlesim/Kill");
+ return 1;
+ }
+
+ client = n.serviceClient("spawn");
+ turtlesim::Spawn spawn_srv;
+
+ for(int i = 0; i < 2; i++){
+ if(i == 0){
+ spawn_srv.request.x = 5.0;
+ spawn_srv.request.y = 5.0;
+ spawn_srv.request.name = "stationary_turtle";
+ }
+ else{
+ spawn_srv.request.x = 25.0;
+ spawn_srv.request.y = 10.0;
+ spawn_srv.request.name = "moving_turtle";
+ }
+ spawn_srv.request.theta = 0.0;
+ if (client.call(spawn_srv)){
+ ROS_INFO("Name of new turtle: %s", spawn_srv.response.name.c_str());
+ }
+ else{
+ ROS_ERROR("Failed to call service turtlesim/Spawn");
+ return 1;
+ }
+ }
+
+ ros::ServiceServer service = n.advertiseService("reset_moving_turtle", reset);
+ ros::Subscriber sub = n.subscribe("moving_turtle/pose", 1, poseCallback);
+ ros::Publisher dist_pub = n.advertise("dist_btwn_turtles", 1);
+ ros::Rate loop_rate(30);
+
+ while (ros::ok()){
+ dist_pub.publish(dist_msg);
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+
+ //ros::spin();
+
+ return 0;
+}
diff --git a/software_training_assignment/src/waypoint_action.cpp b/software_training_assignment/src/waypoint_action.cpp
new file mode 100644
index 0000000..4853a8a
--- /dev/null
+++ b/software_training_assignment/src/waypoint_action.cpp
@@ -0,0 +1,53 @@
+#include "waypoint_action.h"
+
+WaypointAction::WaypointAction(std::string name) :
+ server(nh_, name, boost::bind(&WaypointAction::executeCB, this, _1), false), action_name_(name)
+{
+ server.start();
+}
+
+WaypointAction::~WaypointAction(void)
+{
+}
+
+void WaypointAction::executeCB(const actionlib_tutorials::WaypointGoalConstPtr &goal)
+{
+ // helper variables
+ ros::Rate r(1);
+ bool success = true;
+
+ // push_back the seeds for the Waypoint sequence
+ feedback_.sequence.clear();
+ feedback_.sequence.push_back(0);
+ feedback_.sequence.push_back(1);
+
+ // publish info to the console for the user
+ ROS_INFO("%s: Executing, creating Waypoint sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
+
+ // start executing the action
+ for(int i=1; i<=goal->order; i++)
+ {
+ // check that preempt has not been requested by the client
+ if (server.isPreemptRequested() || !ros::ok())
+ {
+ ROS_INFO("%s: Preempted", action_name_.c_str());
+ // set the action state to preempted
+ server.setPreempted();
+ success = false;
+ break;
+ }
+ feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
+ // publish the feedback
+ server.publishFeedback(feedback_);
+ // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
+ r.sleep();
+ }
+
+ if(success)
+ {
+ result_.sequence = feedback_.sequence;
+ ROS_INFO("%s: Succeeded", action_name_.c_str());
+ // set the action state to succeeded
+ server.setSucceeded(result_);
+ }
+}
diff --git a/software_training_assignment/src/waypoint_action.h b/software_training_assignment/src/waypoint_action.h
new file mode 100644
index 0000000..46eeb1a
--- /dev/null
+++ b/software_training_assignment/src/waypoint_action.h
@@ -0,0 +1,31 @@
+#include
+#include
+#include "software_training_assignment/WaypointAction.h"
+
+#ifndef WAYPOINT_ACTION_H
+#define WAYPOINT_ACTION_H
+
+class WaypointAction
+{
+protected:
+
+ ros::NodeHandle nh_;
+ ros::Publisher twist_pub;
+ actionlib::SimpleActionServer server; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
+ std::string action_name_;
+ // create messages that are used to published feedback/result
+ actionlib_tutorials::WaypointFeedback feedback_;
+ actionlib_tutorials::WaypointResult result_;
+
+
+public:
+
+ WaypointAction(std::string name) :
+ server(nh_, name, boost::bind(&WaypointAction::executeCB, this, _1), false), action_name_(name);
+
+ ~WaypointAction(void);
+
+ void executeCB(const actionlib_tutorials::WaypointGoalConstPtr &goal);
+};
+
+#endif
diff --git a/software_training_assignment/srv/Reset.srv b/software_training_assignment/srv/Reset.srv
new file mode 100644
index 0000000..410e0f9
--- /dev/null
+++ b/software_training_assignment/srv/Reset.srv
@@ -0,0 +1,2 @@
+---
+bool success