From 62f5fd8950891a877f669333b36f2ee485b09a64 Mon Sep 17 00:00:00 2001 From: Saman Sandhu Date: Sun, 30 Aug 2020 16:01:00 -0400 Subject: [PATCH 1/5] Software Challenge Code --- .gitignore | 0 CMakeLists.txt | 69 ++++++ software_training_assignment/CMakeLists.txt | 226 ++++++++++++++++++ .../action/MoveTurtle.action | 9 + .../launch/launcher.launch | 11 + software_training_assignment/msg/Distance.msg | 4 + software_training_assignment/package.xml | 68 ++++++ .../src/DistancePublisher.cpp | 28 +++ .../src/DistancePublisher.h | 25 ++ .../src/MoveTurtleActionTester.cpp | 41 ++++ .../src/MovingTurtle.cpp | 88 +++++++ .../src/MovingTurtle.h | 47 ++++ .../src/StationaryTurtle.cpp | 10 + .../src/StationaryTurtle.h | 17 ++ software_training_assignment/src/Turtle.cpp | 51 ++++ software_training_assignment/src/Turtle.h | 39 +++ software_training_assignment/src/main.cpp | 35 +++ .../srv/ResetMovingTurtle.srv | 2 + 18 files changed, 770 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 software_training_assignment/CMakeLists.txt create mode 100644 software_training_assignment/action/MoveTurtle.action create mode 100644 software_training_assignment/launch/launcher.launch create mode 100644 software_training_assignment/msg/Distance.msg create mode 100644 software_training_assignment/package.xml create mode 100644 software_training_assignment/src/DistancePublisher.cpp create mode 100644 software_training_assignment/src/DistancePublisher.h create mode 100644 software_training_assignment/src/MoveTurtleActionTester.cpp create mode 100644 software_training_assignment/src/MovingTurtle.cpp create mode 100644 software_training_assignment/src/MovingTurtle.h create mode 100644 software_training_assignment/src/StationaryTurtle.cpp create mode 100644 software_training_assignment/src/StationaryTurtle.h create mode 100644 software_training_assignment/src/Turtle.cpp create mode 100644 software_training_assignment/src/Turtle.h create mode 100644 software_training_assignment/src/main.cpp create mode 100644 software_training_assignment/srv/ResetMovingTurtle.srv diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e69de29 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..cd58121 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,69 @@ +# toplevel CMakeLists.txt for a catkin workspace +# catkin/cmake/toplevel.cmake + +cmake_minimum_required(VERSION 3.0.2) + +project(Project) + +set(CATKIN_TOPLEVEL TRUE) + +# search for catkin within the workspace +set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") +execute_process(COMMAND ${_cmd} + RESULT_VARIABLE _res + OUTPUT_VARIABLE _out + ERROR_VARIABLE _err + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_STRIP_TRAILING_WHITESPACE +) +if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) + # searching fot catkin resulted in an error + string(REPLACE ";" " " _cmd_str "${_cmd}") + message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") +endif() + +# include catkin from workspace or via find_package() +if(_res EQUAL 0) + set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") + # include all.cmake without add_subdirectory to let it operate in same scope + include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) + add_subdirectory("${_out}") + +else() + # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument + # or CMAKE_PREFIX_PATH from the environment + if(NOT DEFINED CMAKE_PREFIX_PATH) + if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") + if(NOT WIN32) + string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + else() + set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + endif() + endif() + endif() + + # list of catkin workspaces + set(catkin_search_path "") + foreach(path ${CMAKE_PREFIX_PATH}) + if(EXISTS "${path}/.catkin") + list(FIND catkin_search_path ${path} _index) + if(_index EQUAL -1) + list(APPEND catkin_search_path ${path}) + endif() + endif() + endforeach() + + # search for catkin in all workspaces + set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) + find_package(catkin QUIET + NO_POLICY_SCOPE + PATHS ${catkin_search_path} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + unset(CATKIN_TOPLEVEL_FIND_PACKAGE) + + if(NOT catkin_FOUND) + message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") + endif() +endif() + +catkin_workspace() diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt new file mode 100644 index 0000000..402dd6e --- /dev/null +++ b/software_training_assignment/CMakeLists.txt @@ -0,0 +1,226 @@ +cmake_minimum_required(VERSION 3.0.2) +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 + rospy + turtlesim + std_msgs + 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 + ResetMovingTurtle.srv +) + +## Generate actions in the 'action' folder +add_action_files( + DIRECTORY action + FILES + MoveTurtle.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 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 + /src + ${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 +# add_executable(${PROJECT_NAME}_node src/software_training_assignment_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} +# ) + +set(SOURCE_FILES + src/main.cpp + src/Turtle.cpp + src/StationaryTurtle.cpp + src/MovingTurtle.cpp + src/DistancePublisher.cpp +) + +add_executable(my_turtle ${SOURCE_FILES}) +target_link_libraries(my_turtle ${catkin_LIBRARIES}) +add_dependencies(my_turtle software_training_assignment_generate_messages_cpp) + +add_executable(MoveTurtleActionTester src/MoveTurtleActionTester.cpp) +target_link_libraries(MoveTurtleActionTester ${catkin_LIBRARIES}) +add_dependencies(MoveTurtleActionTester software_training_assignment_generate_messages_cpp) + +############# +## 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_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/MoveTurtle.action b/software_training_assignment/action/MoveTurtle.action new file mode 100644 index 0000000..e5e0e67 --- /dev/null +++ b/software_training_assignment/action/MoveTurtle.action @@ -0,0 +1,9 @@ +# goal definition +float32 x +float32 y +--- +# result definition +duration time +--- +# feedback +float32 distance \ No newline at end of file diff --git a/software_training_assignment/launch/launcher.launch b/software_training_assignment/launch/launcher.launch new file mode 100644 index 0000000..082414b --- /dev/null +++ b/software_training_assignment/launch/launcher.launch @@ -0,0 +1,11 @@ + + + + 128 + 128 + 0 + + + + + \ No newline at end of file diff --git a/software_training_assignment/msg/Distance.msg b/software_training_assignment/msg/Distance.msg new file mode 100644 index 0000000..e2953b6 --- /dev/null +++ b/software_training_assignment/msg/Distance.msg @@ -0,0 +1,4 @@ +float32 x +float32 y + +float32 distance \ No newline at end of file diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml new file mode 100644 index 0000000..65a87b9 --- /dev/null +++ b/software_training_assignment/package.xml @@ -0,0 +1,68 @@ + + + software_training_assignment + 0.0.0 + The software_training_assignment package + + + + + ssandhu + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + message_generation + actionlib + actionlib_msgs + message_runtime + + + + + + + + + diff --git a/software_training_assignment/src/DistancePublisher.cpp b/software_training_assignment/src/DistancePublisher.cpp new file mode 100644 index 0000000..a9d63d5 --- /dev/null +++ b/software_training_assignment/src/DistancePublisher.cpp @@ -0,0 +1,28 @@ +#include "DistancePublisher.h" + +#include "software_training_assignment/Distance.h" + +#include + +// ctor +DistancePublisher::DistancePublisher(ros::NodeHandle &n, Turtle* t1, Turtle* t2) : n_{n}, t1_{t1}, t2_{t2} { + // init ros distance publisher + dist_pub_ = n_.advertise("distance_publisher", 1000); +} + +void DistancePublisher::publish() { + turtlesim::Pose t1_Position = t1_->getPosition(); + turtlesim::Pose t2_Position = t2_->getPosition(); + + // set message + software_training_assignment::Distance msg; + msg.x = t1_Position.x - t2_Position.x; + msg.y = t1_Position.y - t2_Position.y; + msg.distance = sqrt(pow(msg.x, 2) + pow(msg.y, 2)); + + ROS_INFO_STREAM(std::setprecision(2) << std::fixed << " distance: " << msg.x << " " << msg.y << " " << msg.distance); + + // publish + dist_pub_.publish(msg); + ros::spinOnce(); +} diff --git a/software_training_assignment/src/DistancePublisher.h b/software_training_assignment/src/DistancePublisher.h new file mode 100644 index 0000000..1b506f0 --- /dev/null +++ b/software_training_assignment/src/DistancePublisher.h @@ -0,0 +1,25 @@ +#ifndef DISTANCE_PUBLISHER_H +#define DISTANCE_PUBLISHER_H + +#include "Turtle.h" + +class DistancePublisher { + public: + // ctor + DistancePublisher(ros::NodeHandle &n, Turtle* t1, Turtle* t2); + + // publishes distance + void publish(); + + private: + ros::NodeHandle n_; + + // ros publisher + ros::Publisher dist_pub_; + + // 2 Turtle's to compare + Turtle* t1_; + Turtle* t2_; +}; + +#endif diff --git a/software_training_assignment/src/MoveTurtleActionTester.cpp b/software_training_assignment/src/MoveTurtleActionTester.cpp new file mode 100644 index 0000000..ec4790b --- /dev/null +++ b/software_training_assignment/src/MoveTurtleActionTester.cpp @@ -0,0 +1,41 @@ +#include "ros/ros.h" + +#include +#include + +#include "software_training_assignment/MoveTurtleAction.h" + +int main(int argc, char **argv) { + + ros::init(argc, argv, "move_turtle_action_tester"); + + ros::NodeHandle n; + + float x, y; + n.getParam("/move_turtle_action_tester/x", x); + n.getParam("/move_turtle_action_tester/y", y); + + actionlib::SimpleActionClient actionClient("move_turtle_action_server", true); + + ROS_INFO("Waiting for action server to start."); + actionClient.waitForServer(); // will wait for infinite time + + // send a goal to the action + ROS_INFO("Action server started, sending goal."); + software_training_assignment::MoveTurtleGoal goal; + goal.x = x; + goal.y = y; + actionClient.sendGoal(goal); + + // wait for the action to return + bool finished_before_timeout = actionClient.waitForResult(ros::Duration(30.0)); + + if (finished_before_timeout) { + actionlib::SimpleClientGoalState state = actionClient.getState(); + ROS_INFO("Action finished: %s", state.toString().c_str()); + } else { + ROS_INFO("Action did not finish before the time out."); + } + + return 0; +} diff --git a/software_training_assignment/src/MovingTurtle.cpp b/software_training_assignment/src/MovingTurtle.cpp new file mode 100644 index 0000000..04cfa2c --- /dev/null +++ b/software_training_assignment/src/MovingTurtle.cpp @@ -0,0 +1,88 @@ +#include "MovingTurtle.h" + +#include "turtlesim/Pose.h" + +#define _USE_MATH_DEFINES +#include + +MovingTurtle* MovingTurtle::getInstance(ros::NodeHandle &n) { + // lazy instantiation + static MovingTurtle instance_(n); + return &instance_; +} + +// ctor +MovingTurtle::MovingTurtle(ros::NodeHandle &n) : + Turtle(n, 25, 10, "moving_turtle"), + resetService_(n.advertiseService("reset_moving_turtle", &MovingTurtle::_resetCallback, this)), + cmd_vel_pub_(n.advertise(name_ + "/cmd_vel", 1000)), + actionServer_(n, "move_turtle_action_server", boost::bind(&MovingTurtle::_moveCallBack, this, _1), false) { + + // start action server + actionServer_.start(); +} + +bool MovingTurtle::_resetCallback(software_training_assignment::ResetMovingTurtle::Request &req, software_training_assignment::ResetMovingTurtle::Response &res) { + // init teleport service client + ros::ServiceClient teleportClient = n_.serviceClient(name_ + "/teleport_absolute"); + turtlesim::TeleportAbsolute teleportSrv; + + // set request to starting position + teleportSrv.request.x = startX_; + teleportSrv.request.y = startY_; + teleportSrv.request.theta = 0; + + // attempt teleportation + res.success = teleportClient.call(teleportSrv); + return res.success; +} + +void MovingTurtle::_moveCallBack(const software_training_assignment::MoveTurtleGoalConstPtr &goal) { + ros::Rate loop_rate(1); + + ros::Time startTime = ros::Time::now(); + + // setup message parameters + geometry_msgs::Twist msg; + msg.linear.y = 0; + msg.linear.z = 0; + msg.angular.x = 0; + msg.angular.y = 0; + + turtlesim::Pose currPosition = getPosition(); + float deltaX = goal->x - currPosition.x; + float deltaY = goal->y - currPosition.y; + + // publish rotation request + + float angleToGoal = atan(deltaY / deltaX); + if (deltaX >= 0) { + angleToGoal -= currPosition.theta; + } else { + angleToGoal -= (currPosition.theta - M_PI); + } + + msg.linear.x = 0; + msg.angular.z = angleToGoal; + cmd_vel_pub_.publish(msg); + + loop_rate.sleep(); + + // publish movement request + + float distanceToGoal = sqrt(pow(deltaX, 2) + pow(deltaY, 2)); + + msg.linear.x = distanceToGoal; + msg.angular.z = 0; + cmd_vel_pub_.publish(msg); + + loop_rate.sleep(); + + // set result and feedback + result_.time = ros::Time::now() - startTime; + feedback_.distance = distanceToGoal; + + // publish action feedback, and result + actionServer_.publishFeedback(feedback_); + actionServer_.setSucceeded(result_); +} diff --git a/software_training_assignment/src/MovingTurtle.h b/software_training_assignment/src/MovingTurtle.h new file mode 100644 index 0000000..1e6d8f5 --- /dev/null +++ b/software_training_assignment/src/MovingTurtle.h @@ -0,0 +1,47 @@ +#ifndef MOVING_TURTLE_H +#define MOVING_TURTLE_H + +#include "ros/ros.h" + +#include + +#include "turtlesim/TeleportAbsolute.h" + +#include + +#include "software_training_assignment/ResetMovingTurtle.h" +#include "software_training_assignment/MoveTurtleAction.h" + +#include "Turtle.h" + +// Singleton "moving_turtle" class +class MovingTurtle : public Turtle { + public: + // instance accessor + static MovingTurtle* getInstance(ros::NodeHandle &n); + + private: + // moving turtle reset service + ros::ServiceServer resetService_; + + // moving turtle cmd_vel publisher + ros::Publisher cmd_vel_pub_; + + // moving turtle action server + actionlib::SimpleActionServer actionServer_; + + // action parameters + software_training_assignment::MoveTurtleResult result_; + software_training_assignment::MoveTurtleFeedback feedback_; + + // ctor + MovingTurtle(ros::NodeHandle &n); + + // reset service callback + bool _resetCallback(software_training_assignment::ResetMovingTurtle::Request &req, software_training_assignment::ResetMovingTurtle::Response &res); + + // move action callback + void _moveCallBack(const software_training_assignment::MoveTurtleGoalConstPtr &goal); +}; + +#endif diff --git a/software_training_assignment/src/StationaryTurtle.cpp b/software_training_assignment/src/StationaryTurtle.cpp new file mode 100644 index 0000000..4cea4cf --- /dev/null +++ b/software_training_assignment/src/StationaryTurtle.cpp @@ -0,0 +1,10 @@ +#include "StationaryTurtle.h" + +StationaryTurtle* StationaryTurtle::getInstance(ros::NodeHandle &n) { + // lazy instantiation + static StationaryTurtle instance_(n); + return &instance_; +} + +// ctor +StationaryTurtle::StationaryTurtle(ros::NodeHandle &n) : Turtle(n, 5, 5, "stationary_turtle") {} diff --git a/software_training_assignment/src/StationaryTurtle.h b/software_training_assignment/src/StationaryTurtle.h new file mode 100644 index 0000000..2df95bf --- /dev/null +++ b/software_training_assignment/src/StationaryTurtle.h @@ -0,0 +1,17 @@ +#ifndef STATIONARY_TURTLE_H +#define STATIONARY_TURTLE_H + +#include "Turtle.h" + +// Singleton "stationary_turtle" class +class StationaryTurtle : public Turtle { + public: + // instance accessor + static StationaryTurtle* getInstance(ros::NodeHandle &n); + + private: + // ctor + StationaryTurtle(ros::NodeHandle &n); +}; + +#endif diff --git a/software_training_assignment/src/Turtle.cpp b/software_training_assignment/src/Turtle.cpp new file mode 100644 index 0000000..e0d652e --- /dev/null +++ b/software_training_assignment/src/Turtle.cpp @@ -0,0 +1,51 @@ +#include "Turtle.h" + +// ctor +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); +} diff --git a/software_training_assignment/src/Turtle.h b/software_training_assignment/src/Turtle.h new file mode 100644 index 0000000..64334d6 --- /dev/null +++ b/software_training_assignment/src/Turtle.h @@ -0,0 +1,39 @@ +#ifndef TURTLE_H +#define 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(); +}; + +#endif diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp new file mode 100644 index 0000000..339d52c --- /dev/null +++ b/software_training_assignment/src/main.cpp @@ -0,0 +1,35 @@ +#include "ros/ros.h" + +#include "Turtle.h" +#include "StationaryTurtle.h" +#include "MovingTurtle.h" + +#include "DistancePublisher.h" + +int main(int argc, char **argv) { + + ros::init(argc, argv, "main"); + + ros::NodeHandle n; + ros::Rate loop_rate(1); + + // kill current turtle loop + while (!Turtle::kill(n, "turtle1")) { + loop_rate.sleep(); + ROS_ERROR("Failed to clear all turtles"); + } + ROS_INFO("Cleared all turtles"); + + // spawn new turtles + StationaryTurtle* stationaryTurtle = StationaryTurtle::getInstance(n); + MovingTurtle* movingTurtle = MovingTurtle::getInstance(n); + + // distance publisher + DistancePublisher dist_pub(n, stationaryTurtle, movingTurtle); + while(ros::ok()) { + dist_pub.publish(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/software_training_assignment/srv/ResetMovingTurtle.srv b/software_training_assignment/srv/ResetMovingTurtle.srv new file mode 100644 index 0000000..1308cc2 --- /dev/null +++ b/software_training_assignment/srv/ResetMovingTurtle.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file From 6266a5b59ac6f99a1b14d06bbb93ee531608f6ba Mon Sep 17 00:00:00 2001 From: Saman Sandhu <46453550+ss25sand@users.noreply.github.com> Date: Sun, 30 Aug 2020 16:16:02 -0400 Subject: [PATCH 2/5] Create README.md --- README.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 77bcc77..9d464ac 100644 --- a/README.md +++ b/README.md @@ -1 +1,14 @@ -# software_challenge \ No newline at end of file +# software_challenge + +## Usage: +1. Add package to your workspace, `cd` into your workspace, run `source ./devel/setup.bash`, `catkin_make` +2. Start the turtlesim and my_turtle nodes by running: + `roslaunch software_training_assignment launcher.launch` +3. To use the reset "moving_turtle" service run: + `rosservice call /reset_moving_turtle` +4. To get information about the distances between the "stationary_turtle" and "moving_turtle" run: + `rostopic echo /turtle_distance` +5. To use the "moving_turtle" action run: + `rosrun software_training_assignment MoveTurtleActionTester _x:=2 _y:=5` + +Replace the x and y parameters to whatever you would like to move the "moving_turtle". From 057e2ed26ae30a679500e1ed67daf179971d8694 Mon Sep 17 00:00:00 2001 From: Saman Sandhu Date: Sun, 30 Aug 2020 16:18:13 -0400 Subject: [PATCH 3/5] Some renaming --- software_training_assignment/launch/launcher.launch | 2 +- software_training_assignment/src/DistancePublisher.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/software_training_assignment/launch/launcher.launch b/software_training_assignment/launch/launcher.launch index 082414b..9000fa9 100644 --- a/software_training_assignment/launch/launcher.launch +++ b/software_training_assignment/launch/launcher.launch @@ -1,6 +1,6 @@ - + 128 128 0 diff --git a/software_training_assignment/src/DistancePublisher.cpp b/software_training_assignment/src/DistancePublisher.cpp index a9d63d5..626eb4e 100644 --- a/software_training_assignment/src/DistancePublisher.cpp +++ b/software_training_assignment/src/DistancePublisher.cpp @@ -7,7 +7,7 @@ // ctor DistancePublisher::DistancePublisher(ros::NodeHandle &n, Turtle* t1, Turtle* t2) : n_{n}, t1_{t1}, t2_{t2} { // init ros distance publisher - dist_pub_ = n_.advertise("distance_publisher", 1000); + dist_pub_ = n_.advertise("turtle_distance", 1000); } void DistancePublisher::publish() { From bf5545ae3ef3813e45d5641b95597fed13b07ce5 Mon Sep 17 00:00:00 2001 From: Saman Sandhu Date: Mon, 31 Aug 2020 15:28:12 -0400 Subject: [PATCH 4/5] Removed extra comments --- software_training_assignment/CMakeLists.txt | 145 +------------------- software_training_assignment/package.xml | 52 +------ 2 files changed, 4 insertions(+), 193 deletions(-) diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt index 402dd6e..d17c925 100644 --- a/software_training_assignment/CMakeLists.txt +++ b/software_training_assignment/CMakeLists.txt @@ -5,8 +5,6 @@ project(software_training_assignment) 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 rospy @@ -17,39 +15,10 @@ find_package(catkin REQUIRED COMPONENTS 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 @@ -64,7 +33,6 @@ add_service_files( ## Generate actions in the 'action' folder add_action_files( - DIRECTORY action FILES MoveTurtle.action ) @@ -76,35 +44,10 @@ generate_messages( 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 @@ -117,43 +60,12 @@ catkin_package( ########### ## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include /src ${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 -# add_executable(${PROJECT_NAME}_node src/software_training_assignment_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} -# ) - set(SOURCE_FILES src/main.cpp src/Turtle.cpp @@ -169,58 +81,3 @@ add_dependencies(my_turtle software_training_assignment_generate_messages_cpp) add_executable(MoveTurtleActionTester src/MoveTurtleActionTester.cpp) target_link_libraries(MoveTurtleActionTester ${catkin_LIBRARIES}) add_dependencies(MoveTurtleActionTester software_training_assignment_generate_messages_cpp) - -############# -## 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_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/package.xml b/software_training_assignment/package.xml index 65a87b9..0d931bf 100644 --- a/software_training_assignment/package.xml +++ b/software_training_assignment/package.xml @@ -1,54 +1,15 @@ + software_training_assignment 0.0.0 The software_training_assignment package - - - - ssandhu - + Sam Sandhu - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + catkin roscpp rospy @@ -58,11 +19,4 @@ actionlib_msgs message_runtime - - - - - - - From f77a809794ac237d2f2508ae65c2781a538b0040 Mon Sep 17 00:00:00 2001 From: Saman Sandhu <46453550+ss25sand@users.noreply.github.com> Date: Mon, 31 Aug 2020 15:30:34 -0400 Subject: [PATCH 5/5] Update README.md --- README.md | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 9d464ac..cf498fe 100644 --- a/README.md +++ b/README.md @@ -3,12 +3,20 @@ ## Usage: 1. Add package to your workspace, `cd` into your workspace, run `source ./devel/setup.bash`, `catkin_make` 2. Start the turtlesim and my_turtle nodes by running: - `roslaunch software_training_assignment launcher.launch` + ``` + roslaunch software_training_assignment launcher.launch + ``` 3. To use the reset "moving_turtle" service run: - `rosservice call /reset_moving_turtle` + ``` + rosservice call /reset_moving_turtle + ``` 4. To get information about the distances between the "stationary_turtle" and "moving_turtle" run: - `rostopic echo /turtle_distance` + ``` + rostopic echo /turtle_distance + ``` 5. To use the "moving_turtle" action run: - `rosrun software_training_assignment MoveTurtleActionTester _x:=2 _y:=5` + ``` + rosrun software_training_assignment MoveTurtleActionTester _x:=2 _y:=5 + ``` Replace the x and y parameters to whatever you would like to move the "moving_turtle".