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/README.md b/README.md index 77bcc77..cf498fe 100644 --- a/README.md +++ b/README.md @@ -1 +1,22 @@ -# 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". diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt new file mode 100644 index 0000000..d17c925 --- /dev/null +++ b/software_training_assignment/CMakeLists.txt @@ -0,0 +1,83 @@ +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 +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + turtlesim + std_msgs + message_generation + actionlib + actionlib_msgs +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## 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( + FILES + MoveTurtle.action +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + actionlib_msgs +) + +################################### +## catkin specific configuration ## +################################### + +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 +include_directories( + include + /src + ${catkin_INCLUDE_DIRS} +) + +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) 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..9000fa9 --- /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..0d931bf --- /dev/null +++ b/software_training_assignment/package.xml @@ -0,0 +1,22 @@ + + + + software_training_assignment + 0.0.0 + The software_training_assignment package + + Sam Sandhu + + 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..626eb4e --- /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("turtle_distance", 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