diff --git a/software_training/.catkin_tools/CATKIN_IGNORE b/software_training/.catkin_tools/CATKIN_IGNORE
new file mode 100644
index 0000000..e69de29
diff --git a/software_training/.catkin_tools/README b/software_training/.catkin_tools/README
new file mode 100644
index 0000000..4706f47
--- /dev/null
+++ b/software_training/.catkin_tools/README
@@ -0,0 +1,13 @@
+# Catkin Tools Metadata
+
+This directory was generated by catkin_tools and it contains persistent
+configuration information used by the `catkin` command and its sub-commands.
+
+Each subdirectory of the `profiles` directory contains a set of persistent
+configuration options for separate profiles. The default profile is called
+`default`. If another profile is desired, it can be described in the
+`profiles.yaml` file in this directory.
+
+Please see the catkin_tools documentation before editing any files in this
+directory. Most actions can be performed with the `catkin` command-line
+program.
diff --git a/software_training/.catkin_tools/VERSION b/software_training/.catkin_tools/VERSION
new file mode 100644
index 0000000..7ceb040
--- /dev/null
+++ b/software_training/.catkin_tools/VERSION
@@ -0,0 +1 @@
+0.6.1
\ No newline at end of file
diff --git a/software_training/.catkin_tools/profiles/default/build.yaml b/software_training/.catkin_tools/profiles/default/build.yaml
new file mode 100644
index 0000000..4b2ebed
--- /dev/null
+++ b/software_training/.catkin_tools/profiles/default/build.yaml
@@ -0,0 +1,17 @@
+blacklist: []
+build_space: build
+catkin_make_args: []
+cmake_args: []
+devel_layout: linked
+devel_space: devel
+extend_path: null
+install: false
+install_space: install
+isolate_install: false
+jobs_args: []
+log_space: logs
+make_args: []
+source_space: src
+use_env_cache: false
+use_internal_make_jobserver: true
+whitelist: []
diff --git a/software_training/.catkin_tools/profiles/default/devel_collisions.txt b/software_training/.catkin_tools/profiles/default/devel_collisions.txt
new file mode 100644
index 0000000..e69de29
diff --git a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt
new file mode 100644
index 0000000..0d98b05
--- /dev/null
+++ b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt
@@ -0,0 +1,13 @@
+/home/ariwasch/catkin_ws/src/software_training/build/catkin_tools_prebuild
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.zsh
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.sh /home/ariwasch/catkin_ws/src/software_training/devel/./setup.sh
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/cmake.lock /home/ariwasch/catkin_ws/src/software_training/devel/./cmake.lock
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.zsh /home/ariwasch/catkin_ws/src/software_training/devel/./setup.zsh
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/_setup_util.py /home/ariwasch/catkin_ws/src/software_training/devel/./_setup_util.py
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.bash /home/ariwasch/catkin_ws/src/software_training/devel/./setup.bash
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.bash /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.bash
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/env.sh /home/ariwasch/catkin_ws/src/software_training/devel/./env.sh
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.sh /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.sh
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/ariwasch/catkin_ws/src/software_training/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/ariwasch/catkin_ws/src/software_training/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake
+/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/ariwasch/catkin_ws/src/software_training/devel/lib/pkgconfig/catkin_tools_prebuild.pc
diff --git a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml
new file mode 100644
index 0000000..134c59a
--- /dev/null
+++ b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml
@@ -0,0 +1,10 @@
+
+ catkin_tools_prebuild
+
+ This package is used to generate catkin setup files.
+
+ 0.0.0
+ BSD
+ jbohren
+ catkin
+
diff --git a/software_training/.gitignore b/software_training/.gitignore
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/software_training/.gitignore
@@ -0,0 +1 @@
+
diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt
new file mode 100644
index 0000000..db0d42d
--- /dev/null
+++ b/software_training/CMakeLists.txt
@@ -0,0 +1,192 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(software_training)
+
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+ message_generation
+ actionlib_msgs
+ actionlib
+)
+
+
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+ FILES
+ service.srv
+)
+
+## Generate actions in the 'action' folder
+add_action_files(
+ FILES
+ action.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
+# CATKIN_DEPENDS roscpp rospy std_msgs
+# DEPENDS system_lib
+ CATKIN_DEPENDS
+ roscpp
+ std_msgs
+ message_runtime
+ actionlib_msgs
+)
+
+###########
+## 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.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_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
+# 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.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(addTurtle src/addTurtle.cpp)
+target_link_libraries(addTurtle ${catkin_LIBRARIES})
+
+add_executable(talker src/talker.cpp)
+target_link_libraries(talker ${catkin_LIBRARIES})
+#add_dependencies(talker software_training_generate_messages_cpp)
+
+add_executable(listener src/listener.cpp)
+target_link_libraries(listener ${catkin_LIBRARIES})
+#add_dependencies(listener software_training_generate_messages_cpp)
+
+add_executable(service src/service.cpp)
+target_link_libraries(service ${catkin_LIBRARIES})
+add_dependencies(service software_training_generate_messages_cpp)
+
diff --git a/software_training/action/action.action b/software_training/action/action.action
new file mode 100644
index 0000000..f499ddf
--- /dev/null
+++ b/software_training/action/action.action
@@ -0,0 +1,6 @@
+float32 x
+float32 y
+---
+duration time
+---
+float32 distance
diff --git a/software_training/msg/Distance.msg b/software_training/msg/Distance.msg
new file mode 100644
index 0000000..7254481
--- /dev/null
+++ b/software_training/msg/Distance.msg
@@ -0,0 +1,4 @@
+float32 x
+float32 y
+
+float32 distance
diff --git a/software_training/msg/Num.msg b/software_training/msg/Num.msg
new file mode 100644
index 0000000..9806981
--- /dev/null
+++ b/software_training/msg/Num.msg
@@ -0,0 +1 @@
+int64 num
diff --git a/software_training/package.xml b/software_training/package.xml
new file mode 100644
index 0000000..a4e9faf
--- /dev/null
+++ b/software_training/package.xml
@@ -0,0 +1,74 @@
+
+
+ software_training
+ 0.0.0
+ The software_training package
+
+
+
+
+ ariwasch
+
+
+
+
+
+ BSD
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ turtlesim
+ geometry_msgs
+ rospy
+ std_msgs
+ actionlib_msgs
+ roscpp
+ rospy
+ std_msgs
+ actionlib_msgs
+ actionlib_msgs
+ roscpp
+ rospy
+ std_msgs
+ message_generation
+ message_runtime
+
+
+
+
+
+
+
diff --git a/software_training/src/action.cpp b/software_training/src/action.cpp
new file mode 100644
index 0000000..7c61e1e
--- /dev/null
+++ b/software_training/src/action.cpp
@@ -0,0 +1,50 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "software_training/Distance.h"
+#include
+
+int main (int argc, char **argv)
+{
+ ros::init(argc, argv, "action");
+
+ ros::NodeHandle nh;
+ ros::Publisher pub
+ = nh.advertise("/moving_turtle/cmd_vel", 1000);
+ // create the action client
+ // true causes the client to spin its own thread
+
+ //actionlib::SimpleActionClient("action", true);
+ actionlib::SimpleActionClient actionClient("action", true);
+
+ Server server(n, "action", boost::bind(&execute, _1, &server), false);
+ server.start();
+
+ ROS_INFO("Waiting for action server to start.");
+ // wait for the action server to start
+ actionClient.waitForServer(); //will wait for infinite time
+
+ ROS_INFO("Action server started, sending goal.");
+ // send a goal to the action
+ actionlib_tutorials::FibonacciGoal goal;
+ goal.order = 20;
+ ac.sendGoal(goal);
+
+ //wait for the action to return
+ 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.");
+
+ //exit
+ return 0;
+}
diff --git a/software_training/src/addTurtle.cpp b/software_training/src/addTurtle.cpp
new file mode 100644
index 0000000..dd0045e
--- /dev/null
+++ b/software_training/src/addTurtle.cpp
@@ -0,0 +1,65 @@
+//This program spawns a new turtlesim turtle by calling
+// the appropriate service.
+#include
+//The srv class for the service.
+#include
+#include
+#include
+#include
+int main(int argc, char **argv){
+
+ ros::init(argc, argv, "addTurtle");
+ ros::NodeHandle nh;
+
+//Create a client object for the spawn service. This
+//needs to know the data type of the service and its name.
+
+ ros::ServiceClient spawnClient
+ = nh.serviceClient("spawn");
+ std_srvs::Empty srv;
+ srv.request;
+ ros::ServiceClient reset = nh.serviceClient("clear");
+
+ ros::ServiceClient kill = nh.serviceClient("kill");
+ turtlesim::Kill srv2;
+ srv2.request.name = "turtle1";
+
+ reset.call(srv);
+ kill.call(srv2);
+//Create the request and response objects.
+ turtlesim::Spawn::Request req;
+ turtlesim::Spawn::Response resp;
+
+ req.x = 5;
+ req.y = 5;
+ req.theta = M_PI/2;
+ req.name = "stationary_turtle";
+
+ turtlesim::Spawn::Request req2;
+ turtlesim::Spawn::Response resp2;
+
+ req2.x = 25;
+ req2.y = 10;
+ req2.theta = M_PI/2;
+ req2.name = "moving_turtle";
+
+ ros::service::waitForService("spawn", ros::Duration(5));
+ bool success = spawnClient.call(req,resp);
+ bool success2 = spawnClient.call(req2,resp2);
+
+
+ if(success){
+ ROS_INFO_STREAM("Spawned a turtle named "
+ << resp.name);
+ }else{
+ ROS_ERROR_STREAM("Failed to spawn.");
+ }
+
+ if(success2){
+ ROS_INFO_STREAM("Spawned a turtle named "
+ << resp2.name);
+ }else{
+ ROS_ERROR_STREAM("Failed to spawn.");
+ }
+}
+
diff --git a/software_training/src/listener.cpp b/software_training/src/listener.cpp
new file mode 100644
index 0000000..0c60b11
--- /dev/null
+++ b/software_training/src/listener.cpp
@@ -0,0 +1,58 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the ROS system.
+ */
+void chatterCallback(const std_msgs::String::ConstPtr& msg)
+{
+ ROS_INFO("I heard: [%s]", msg->data.c_str());
+}
+
+int main(int argc, char **argv)
+{
+ /**
+ * The ros::init() function needs to see argc and argv so that it can perform
+ * any ROS arguments and name remapping that were provided at the command line.
+ * For programmatic remappings you can use a different version of init() which takes
+ * remappings directly, but for most command-line programs, passing argc and argv is
+ * the easiest way to do it. The third argument to init() is the name of the node.
+ *
+ * You must call one of the versions of ros::init() before using any other
+ * part of the ROS system.
+ */
+ ros::init(argc, argv, "listener");
+
+ /**
+ * NodeHandle is the main access point to communications with the ROS system.
+ * The first NodeHandle constructed will fully initialize this node, and the last
+ * NodeHandle destructed will close down the node.
+ */
+ ros::NodeHandle n;
+
+ /**
+ * The subscribe() call is how you tell ROS that you want to receive messages
+ * on a given topic. This invokes a call to the ROS
+ * master node, which keeps a registry of who is publishing and who
+ * is subscribing. Messages are passed to a callback function, here
+ * called chatterCallback. subscribe() returns a Subscriber object that you
+ * must hold on to until you want to unsubscribe. When all copies of the Subscriber
+ * object go out of scope, this callback will automatically be unsubscribed from
+ * this topic.
+ *
+ * The second parameter to the subscribe() function is the size of the message
+ * queue. If messages are arriving faster than they are being processed, this
+ * is the number of messages that will be buffered up before beginning to throw
+ * away the oldest ones.
+ */
+ ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
+
+ /**
+ * ros::spin() will enter a loop, pumping callbacks. With this version, all
+ * callbacks will be called from within this thread (the main one). ros::spin()
+ * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
+ */
+ ros::spin();
+
+ return 0;
+}
diff --git a/software_training/src/service.cpp b/software_training/src/service.cpp
new file mode 100644
index 0000000..dd7054d
--- /dev/null
+++ b/software_training/src/service.cpp
@@ -0,0 +1,46 @@
+//This program spawns a new turtlesim turtle by calling
+// the appropriate service.
+#include
+//The srv class for the service.
+#include
+#include
+#include "turtlesim/TeleportAbsolute.h"
+
+/*class Service {
+
+public:
+
+ bool resetPosition(){
+ ROS_INFO("RESET");
+ return true;
+ };
+
+}*/
+
+
+ bool resetPosition(software_training::ResetTurtle::Request &req, software_training::ResetTurtle::Response &res){
+ ROS_INFO("RESET");
+ ros::NodeHandle nh;
+ turtlesim::TeleportAbsolute srv;
+ srv.request.x = 20
+ srv.request.y = 5
+
+ client = nh.serviceClient("moving_turtle/teleport_absolute");
+ if(client.call(srv) == false){
+ return false;
+ }
+ return true;
+
+
+ return true;
+ };
+int main(int argc, char **argv){
+ ros::init(argc, argv, "service");
+ ros::NodeHandle nh;
+
+ //Service turtle;
+
+ ros::ServiceServer reset = nh.advertise("service", resetPosition);
+
+ ros::spin();
+}
diff --git a/software_training/src/talker.cpp b/software_training/src/talker.cpp
new file mode 100644
index 0000000..b7850a5
--- /dev/null
+++ b/software_training/src/talker.cpp
@@ -0,0 +1,116 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include "geometry_msgs/Twist.h"
+#include
+#include
+#include "software_training/Distance.h"
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+
+turtlesim::Pose pos1
+turtlesim::Pose pos2
+
+void chatterCallBack(const std_msgs::String::ConstPtr& msg)
+{
+ pos1.x = msg->x;
+ pos1.y = msg->y;
+ ROS_INFO("I heard: [%s]", msg->data.c_str());
+}
+
+void chatterCallBack2(const std_msgs::String::ConstPtr& msg)
+{
+ pos2.x = msg->x;
+ pos2.y = msg->y;
+ ROS_INFO("I heard: [%s]", msg->data.c_str());
+}
+
+int main(int argc, char **argv)
+{
+ /**
+ * The ros::init() function needs to see argc and argv so that it can perform
+ * any ROS arguments and name remapping that were provided at the command line.
+ * For programmatic remappings you can use a different version of init() which takes
+ * remappings directly, but for most command-line programs, passing argc and argv is
+ * the easiest way to do it. The third argument to init() is the name of the node.
+ *
+ * You must call one of the versions of ros::init() before using any other
+ * part of the ROS system.
+ */
+ ros::init(argc, argv, "talker");
+
+ /**
+ * NodeHandle is the main access point to communications with the ROS system.
+ * The first NodeHandle constructed will fully initialize this node, and the last
+ * NodeHandle destructed will close down the node.
+ */
+ ros::NodeHandle n;
+
+ /**
+ * The advertise() function is how you tell ROS that you want to
+ * publish on a given topic name. This invokes a call to the ROS
+ * master node, which keeps a registry of who is publishing and who
+ * is subscribing. After this advertise() call is made, the master
+ * node will notify anyone who is trying to subscribe to this topic name,
+ * and they will in turn negotiate a peer-to-peer connection with this
+ * node. advertise() returns a Publisher object which allows you to
+ * publish messages on that topic through a call to publish(). Once
+ * all copies of the returned Publisher object are destroyed, the topic
+ * will be automatically unadvertised.
+ *
+ * The second parameter to advertise() is the size of the message queue
+ * used for publishing messages. If messages are published more quickly
+ * than we can send them, the number here specifies how many messages to
+ * buffer up before throwing some away.
+ */
+ //ros::Publisher chatter_pub = n.advertise("chatter", 1000);
+ ros::Publisher chatter_pub = n.advertise("/moving_turtle/cmd_vel", 1);
+
+ ros::Subscriber turtle1 = n.subscribe("/stationary_turtle/pose",1000, chatterCallBack);
+ ros::Subscriber turtle2 = n.subscribe("/moving_turtle/pose", 1000, chatterCallBack2);
+
+
+ ros::Rate loop_rate(10);
+ geometry_msgs::Twist move_msg;
+
+ /**
+ * A count of how many messages we have sent. This is used to create
+ * a unique string for each message.
+ */
+ int count = 0;
+ while (ros::ok())
+ {
+ /**
+ * This is a message object. You stuff it with data, and then publish it.
+ */
+ std_msgs::String msg;
+
+ software_training::Distance msg;
+ msg.x = pos1.x - pos2.x;
+ msg.y = pos1.y - pos2.y;
+ msg.distance = sqrt(pow(msg.x,2) + pow(msg.y, 2));
+ std::stringstream ss;
+ //ss << "hello world " << count;
+ //msg.data = ss.str();
+
+ //ROS_INFO("%s", msg.data.c_str());
+
+ /**
+ * The publish() function is how you send messages. The parameter
+ * is the message object. The type of this object must agree with the type
+ * given as a template parameter to the advertise<>() call, as was done
+ * in the constructor above.
+ */
+ chatter_pub.publish(msg);
+
+ ros::spinOnce();
+
+ loop_rate.sleep();
+ ++count;
+ }
+
+
+ return 0;
+}
+
diff --git a/software_training/src/trainingNode.cpp b/software_training/src/trainingNode.cpp
new file mode 100644
index 0000000..c661f71
--- /dev/null
+++ b/software_training/src/trainingNode.cpp
@@ -0,0 +1,91 @@
+#include "Turtle.h"
+
+#include "ros/ros.h"
+
+#include "turtlesim/Kill.h"
+#include "turtlesim/Spawn.h"
+#include "turtlesim/Pose.h"
+
+class Turtle {
+ public:
+ // ctor
+ Turtle(ros::NodeHandle &n, float x, float y, std::string name);
+
+ // kill `name` turtle
+ static bool kill(ros::NodeHandle &n, std::string name);
+
+ // turtle position accessors
+ turtlesim::Pose getPosition() const;
+
+ protected:
+ ros::NodeHandle &n_;
+
+ float startX_;
+ float startY_;
+ std::string name_;
+
+ // position subscriber
+ ros::Subscriber poseSubscriber_;
+ turtlesim::Pose currentPosition_;
+
+ // position subscriber callback
+ void _positionCallback(const turtlesim::Pose &msgIn);
+
+ // spawn `this` Turtle
+ bool _spawn();
+};
+
+Turtle::Turtle(ros::NodeHandle &n, float x, float y, std::string name) : n_{n}, startX_{x}, startY_{y}, name_{name} {
+ // spawn the turtle
+ if (_spawn()) {
+ ROS_INFO("Spawned new %s", name.c_str());
+ } else {
+ ROS_ERROR("Failed to spawn new %s", name.c_str());
+ }
+
+ // init position subscriber
+ poseSubscriber_ = n_.subscribe(name_ + "/pose", 1000, &Turtle::_positionCallback, this);
+}
+
+bool Turtle::kill(ros::NodeHandle &n, std::string name) {
+ // init kill service client
+ ros::ServiceClient killClient = n.serviceClient("kill");
+ turtlesim::Kill killSrv;
+
+ killSrv.request.name = name;
+
+ // attempt to kill turtle
+ return killClient.call(killSrv);
+}
+
+// Update and return current position
+turtlesim::Pose Turtle::getPosition() const {
+ ros::spinOnce();
+ return currentPosition_;
+}
+
+void Turtle::_positionCallback(const turtlesim::Pose &msgIn) {
+ ROS_INFO_STREAM(std::setprecision(2) << std::fixed << name_ << " position: " << msgIn.x << ", " << msgIn.y);
+ currentPosition_ = msgIn;
+}
+
+bool Turtle::_spawn() {
+ // init spawning service client
+ ros::ServiceClient spawnClient = n_.serviceClient("spawn");
+ turtlesim::Spawn spawnSrv;
+
+ // set request
+ spawnSrv.request.x = startX_;
+ spawnSrv.request.y = startY_;
+ spawnSrv.request.theta = 0;
+ spawnSrv.request.name = name_;
+
+ // attempt to spawn turtle
+ return spawnClient.call(spawnSrv);
+}
+
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "trainingNode");
+
+}
diff --git a/software_training/srv/service.srv b/software_training/srv/service.srv
new file mode 100644
index 0000000..410e0f9
--- /dev/null
+++ b/software_training/srv/service.srv
@@ -0,0 +1,2 @@
+---
+bool success