diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt
new file mode 100644
index 0000000..28fb567
--- /dev/null
+++ b/software_training_assignment/CMakeLists.txt
@@ -0,0 +1,226 @@
+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
+ genmsg
+ actionlib_msgs
+ actionlib
+)
+
+## 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
+ Disp.msg
+)
+
+## Generate services in the 'srv' folder
+#add_service_files(FILES
+# MyReset.srv
+#)
+
+## Generate actions in the 'action' folder
+add_action_files(DIRECTORY action
+ FILES
+ Move.action
+# Action2.action
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ std_msgs # Or other packages containing 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
+# DEPENDS system_lib
+ CATKIN_DEPENDS message_runtime
+ actionlib_msgs
+ # actionlib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include
+ ${catkin_INCLUDE_DIRS}
+ ${Boost_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 /home/bahar/catkin_ws/src/software_training_assignment/src/software_training_assignment_node.cpp)
+
+#add_executable(make_action_node src/make_action.cpp)
+
+add_executable(simple_action_client /home/bahar/catkin_ws/src/software_training_assignment/src/simple_action_client.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})
+
+#add_dependencies(make_action_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(simple_action_client ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+ ${catkin_LIBRARIES}
+)
+
+#target_link_libraries(make_action_node
+ # ${catkin_LIBRARIES}
+#)
+
+target_link_libraries(simple_action_client
+ ${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)
diff --git a/software_training_assignment/action/Move.action b/software_training_assignment/action/Move.action
new file mode 100644
index 0000000..e5317d0
--- /dev/null
+++ b/software_training_assignment/action/Move.action
@@ -0,0 +1,8 @@
+# goal definition
+float64[] endLocation
+---
+# result definition
+float64 time
+---
+# feedback
+float64 distance
diff --git a/software_training_assignment/launch/turtlesim.launch b/software_training_assignment/launch/turtlesim.launch
new file mode 100644
index 0000000..6aa9cef
--- /dev/null
+++ b/software_training_assignment/launch/turtlesim.launch
@@ -0,0 +1,5 @@
+
+
+
+
+
diff --git a/software_training_assignment/msg/Disp.msg b/software_training_assignment/msg/Disp.msg
new file mode 100644
index 0000000..1b76a06
--- /dev/null
+++ b/software_training_assignment/msg/Disp.msg
@@ -0,0 +1,3 @@
+float64 xDisp
+float64 yDisp
+float64 disp
diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml
new file mode 100644
index 0000000..86ce065
--- /dev/null
+++ b/software_training_assignment/package.xml
@@ -0,0 +1,70 @@
+
+
+ software_training_assignment
+ 0.0.0
+ The software_training_assignment package
+
+
+
+
+ bahar
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ roscpp
+ roscpp
+
+ message_generation
+
+
+
+
+
+ message_runtime
+
+
+
+
+
+
+
+ turtlesim
+ turtlesim
+ std_msgs
+ std_msgs
+ catkin
+
+ actionlib
+ actionlib_msgs
+
+
+
+
+
+
+
diff --git a/software_training_assignment/src/simple_action_client.cpp b/software_training_assignment/src/simple_action_client.cpp
new file mode 100644
index 0000000..a98871f
--- /dev/null
+++ b/software_training_assignment/src/simple_action_client.cpp
@@ -0,0 +1,29 @@
+//============================================================================
+// simple_action_client.cpp
+// Bahar Kholdi-Sabeti
+// Feb. 15, 2020
+// simple client for Move action
+// ============================================================================
+#include
+#include
+#include
+
+typedef actionlib::SimpleActionClient Client;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "move_client");
+ Client client ("move", true); // don't need ros::spin()
+ client.waitForServer();
+ software_training_assignment::MoveGoal goal;
+ // define goal
+ goal.endLocation = {5,5}; // enter your waypoint
+
+ client.sendGoal(goal);
+ client.waitForResult(ros::Duration(100.0));
+ if (client.getState()==actionlib::SimpleClientGoalState::SUCCEEDED){
+ printf("Current State: %s\n", client.getState().toString().c_str());
+ }
+ printf("Current State: %s\n", client.getState().toString().c_str());
+ return 0;
+}
diff --git a/software_training_assignment/src/software_training_assignment_node.cpp b/software_training_assignment/src/software_training_assignment_node.cpp
new file mode 100644
index 0000000..3ee283c
--- /dev/null
+++ b/software_training_assignment/src/software_training_assignment_node.cpp
@@ -0,0 +1,293 @@
+//============================================================================
+// software_training_assignment_node.cpp
+// Bahar Kholdi-Sabeti
+// Feb. 15, 2020
+// software training assignment for UW Robotics Team
+
+// Task 4:
+// a) clears any turtles - automatic
+// b) spawns a turtle named stationary_turtle - automatic
+// c) spawns moving_turtle - automatic
+// d) creates a service that resets the moving_turtle - run service called my_reset:
+// in terminal: rosservice call my_reset
+// e) creates publisher - echo coord message
+// in terminal: rostopic echo coord
+// f) creates action that moves moving_turtle to waypoint- run using client node
+// called simple_action_client
+// in terminal: rosrun software_training_assignment simple_action_client
+// You can also echo feedback and result by echoing Move.action
+// in terminal: rostopic echo /move/feedback
+// rostopic echo /move/result
+//
+// can launch this node with rosrun software_training_assignment software_training_assignment_node
+// or launch this AND turtlesim with
+// roslaunch software_training_assignment turtlesim.launch
+//============================================================================
+
+#include "ros/ros.h"
+#include "turtlesim/Color.h"
+#include "turtlesim/Kill.h"
+#include "turtlesim/Spawn.h"
+#include "std_srvs/Empty.h"
+#include "std_msgs/String.h"
+#include "turtlesim/Pose.h"
+#include "math.h"
+#include "software_training_assignment/Disp.h"
+#include
+#include
+#include
+
+float xTurtle1 = -1;
+float yTurtle1 = -1;
+float thetaTurtle1 = -1;
+float xTurtle2 = -1;
+float yTurtle2 = -1;
+float turtleTime = -1; // initialize turtleTime as -1 so that if it's -1 we know that it hasn't been set
+typedef actionlib::SimpleActionServer Server;
+
+// updates moving_turtle's pose
+void poseCallback(const turtlesim::PoseConstPtr& msg)
+{
+ xTurtle1 = msg->x;
+ yTurtle1 = msg->y;
+ thetaTurtle1 = msg->theta;
+}
+
+// updates stationary_turtle's pose
+void poseCallback2(const turtlesim::PoseConstPtr& msg){
+ xTurtle2 = msg->x;
+ yTurtle2 = msg->y;
+}
+
+// resets turtlesim screen
+void reset(){
+ ros::NodeHandle nh;
+ // Wait until the reset service is available
+ ros::service::waitForService("reset");
+ ros::ServiceClient resetClient = nh.serviceClient("/reset");
+ resetClient.waitForExistence();
+ std_srvs::Empty srv;
+ if (!(resetClient.call(srv))){ // calls service, if statement for debugging
+ ROS_ERROR("Error: reset didn't work");
+ }
+}
+
+// kills turtle1
+bool kill(std::string name)
+{
+ ros::NodeHandle nh;
+ // Wait until the kill service is available
+ ros::service::waitForService("kill");
+ ros::ServiceClient killClient = nh.serviceClient("/kill");
+ turtlesim::Kill srv;
+ srv.request.name = name;
+ if (killClient.call(srv)){ // calls service
+ return true;
+ }
+ else{
+ ROS_ERROR("Error: killing didn't work");
+ return false;
+ }
+}
+
+// spawns new turtle
+bool spawn(int x, int y, int theta, std::string name)
+{
+ ros::NodeHandle nh;
+ // Wait until the spawn service is available
+ ros::service::waitForService("spawn");
+ ros::ServiceClient spawnClient = nh.serviceClient("/spawn");
+ turtlesim::Spawn srv;
+ srv.request.x = x;
+ srv.request.y = y;
+ srv.request.theta = theta;
+ srv.request.name = name;
+ if (spawnClient.call(srv)){ // calls service
+ return true;
+ }
+ else{
+ ROS_ERROR("Error: spawn didn't work");
+ return false;
+ }
+}
+
+// kills turtle1 and spawns moving turtle
+bool myReset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
+{
+ ROS_INFO_STREAM("my_reset srv called");
+ if (kill("moving_turtle")){
+ return spawn(25, 10, 0, "moving_turtle");
+ }
+}
+
+// resets moving turtle for part 4d
+// returns whether it was successful or not
+void reset_moving(){
+ ros::NodeHandle nh;
+ ros::ServiceClient my_resetClient = nh.serviceClient("/my_reset");
+ turtlesim::Kill srv;
+ if (!(my_resetClient.call(srv))){ // calls service, if statement for debugging
+ ROS_ERROR("Error: my_reset didn't work");
+ }
+}
+
+// find angle to steer to depending on which quadrant
+// the goal is in
+// could possibly create a bug for angles close to the
+// edges of quadrants
+double findAngle(double xGoal, double yGoal){
+ if (yGoalendLocation[0];
+ double yGoal = goal->endLocation[1];
+ double dist = sqrt(pow((xGoal-xTurtle1), 2)+pow((yGoal-yTurtle1), 2));
+ double steering_angle = findAngle(xGoal, yGoal);
+
+ if (turtleTime == -1){ // initialize time
+ turtleTime = ros::Time::now().toSec();
+ }
+ software_training_assignment::MoveFeedback feedback;
+ software_training_assignment::MoveResult result;
+
+ // a large angle tolerance allows the turtle to have enough time to
+ // decellerate to 0 after it's done turning
+ while (abs(((double)thetaTurtle1)-steering_angle)>0.05){ // where 0.1 is angle tolerance
+ feedback.distance = dist;
+ as->publishFeedback(feedback);
+ // move turtle
+ ros::Publisher velocity_publisher = nh.advertise("/moving_turtle/cmd_vel", 10);
+ geometry_msgs::Twist vel_msg;
+ vel_msg.linear.x = 0;
+ vel_msg.linear.y = 0;
+ vel_msg.linear.z = 0;
+ vel_msg.angular.x = 0;
+ vel_msg.angular.y = 0;
+ vel_msg.angular.z = 0.1; // make it turn hella slow so
+ // we don't have to worry too much about decellerating
+ velocity_publisher.publish(vel_msg);
+ ROS_INFO("Steering angle: %f, turtle angle: %f", steering_angle, ((double)thetaTurtle1));
+ ros::spinOnce();
+ }
+ while (dist>1){ // where 1 is distance tolerance
+ steering_angle = -100;
+ // update moving_turtle location
+ ros::Subscriber mt_pose = nh.subscribe // updates moving turtle pose
+ ("/moving_turtle/pose",10, &poseCallback);
+ // update distance to goal
+ dist = sqrt(pow((xGoal-xTurtle1), 2)+pow((yGoal-yTurtle1), 2));
+ // updates feedback as distance to goal
+ feedback.distance = dist;
+ as->publishFeedback(feedback);
+ // move turtle
+ ros::Publisher velocity_publisher = nh.advertise("/moving_turtle/cmd_vel", 10);
+ geometry_msgs::Twist vel_msg;
+ vel_msg.linear.x = 2;
+ vel_msg.linear.y = 0;
+ vel_msg.linear.z = 0;
+ vel_msg.angular.x = 0;
+ vel_msg.angular.y = 0;
+ vel_msg.angular.z = 0; // decellerates to 0
+ velocity_publisher.publish(vel_msg);
+ ROS_INFO("I am moving, turtle angle: %f", ((double)thetaTurtle1));
+ ros::spinOnce();
+ }
+ // reached goal
+ ROS_INFO("I am at my goal :)");
+ // returns result as time it took to reach goal
+ result.time = ros::Time::now().toSec() - turtleTime;
+ as->setSucceeded(result);
+ turtleTime = -1;
+
+ ros::Publisher velocity_publisher = nh.advertise("/moving_turtle/cmd_vel", 10);
+ geometry_msgs::Twist vel_msg;
+ vel_msg.linear.x = 0;
+ vel_msg.linear.y = 0;
+ vel_msg.linear.z = 0;
+ vel_msg.angular.x = 0;
+ vel_msg.angular.y = 0;
+ vel_msg.angular.z = 0;
+ velocity_publisher.publish(vel_msg);
+ ros::spinOnce();
+}
+
+// updates coord custom msg
+void updateDisp(){
+ ros::NodeHandle nh;
+ ros::Rate loop_rate(10);
+ // publisher for distances between 2 turtles
+ ros::Publisher coord_pub = nh.advertise("coord", 1000);
+
+ // num of sent msgs
+ int count = 0;
+ while (ros::ok())
+ {
+ // updates Disp msg
+ software_training_assignment::Disp msg;
+
+ double xDist = abs(xTurtle2 - xTurtle1);
+ double yDist = abs(yTurtle2 - yTurtle1);
+ double dist = sqrt(pow(xDist, 2)+pow(yDist, 2));
+
+ msg.xDisp = xDist;
+ msg.yDisp = yDist;
+ msg.disp = dist;
+
+ coord_pub.publish(msg);
+ ros::spinOnce();
+ loop_rate.sleep();
+ ++count;
+ }
+}
+
+// main function
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "software_training_assignment_node");
+ ros::NodeHandle nh;
+
+ reset(); // resets everything to start config
+ kill("turtle1"); // kills turtle1 because we don't like him
+ // but also because he's not on the list of instructions
+ spawn(5, 5, 0, "stationary_turtle"); // makes "stationary_turtle" @ (5,5)
+ spawn(25, 10, 0, "moving_turtle"); // makes "moving_turtle" @ (25,10)
+
+ // Register service
+ ros::ServiceServer server = nh.advertiseService("my_reset", &myReset);
+
+ ros::Subscriber mt_pose = nh.subscribe // subscriber for moving turtle
+ ("/moving_turtle/pose",10, &poseCallback);
+ ros::Subscriber st_pose = nh.subscribe // subscriber for stationary turtle
+ ("/stationary_turtle/pose",10, &poseCallback2);
+
+ // set up action
+ ///////////////////////////////////////////////////////////////////////////////////////
+ actionlib::SimpleActionServer as(nh, "move", boost::bind(&execute, _1, &as), false);
+ as.start();
+ ///////////////////////////////////////////////////////////////////////////////////////
+ // done setting up action
+
+ // updates coord custom msg
+ updateDisp();
+
+ return 0;
+}