diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b338363 --- /dev/null +++ b/.gitignore @@ -0,0 +1,21 @@ +# !catkin_ws/ + +# catkin_ws/* +# !catkin_ws/src + +# catkin_ws/src/* +# !catkin_ws/src/software_training_assignment + +# catkin_ws/src/software_training_assignment/* +# !catkin_ws/src/software_training_assignment/src +# !catkin_ws/src/software_training_assignment/CMakeLists.txt + + +README.md + +catkin_ws + +!software_training_assignment +software_training_assignment/* +!software_training_assignment/src +!software_training_assignment/CMakeLists.txt \ No newline at end of file diff --git a/README.md b/README.md deleted file mode 100644 index 77bcc77..0000000 --- a/README.md +++ /dev/null @@ -1 +0,0 @@ -# software_challenge \ No newline at end of file diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt new file mode 100644 index 0000000..d8be523 --- /dev/null +++ b/software_training_assignment/CMakeLists.txt @@ -0,0 +1,207 @@ +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 + turtlesim + 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 + 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 Move.action +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + actionlib_msgs + # Or other packages containing 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 actionlib_msgs +# # DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/software_training_assignment.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# 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} +# ) + +############# +## 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) +add_executable(turtleBehaviour src/turtleBehaviour.cpp) +target_link_libraries(turtleBehaviour ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/software_training_assignment/src/turtleBehaviour.cpp b/software_training_assignment/src/turtleBehaviour.cpp new file mode 100644 index 0000000..60cd623 --- /dev/null +++ b/software_training_assignment/src/turtleBehaviour.cpp @@ -0,0 +1,261 @@ +#include +#include +#include +// #include "ros/console.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void clearTurtles(ros::NodeHandle & n) { + ros::ServiceClient resetClient = n.serviceClient("reset"); + std_srvs::Empty resetSrv; + + bool resetComplete = false; + while (!resetComplete) { + if(resetClient.call(resetSrv)) { + ROS_INFO_STREAM("Completed reset"); + resetComplete = true; + } else { + ROS_INFO_STREAM("Have to make another call"); + } + } + + ros::ServiceClient killClient = n.serviceClient("kill"); + turtlesim::Kill killSrv; + killSrv.request.name = "turtle1"; + if (killClient.call(killSrv)) { + ROS_INFO_STREAM("cleared all turtles"); + } +} + +void spawnTurtles(ros::NodeHandle & n) { + ros::ServiceClient spawnClient = n.serviceClient("spawn"); + turtlesim::Spawn spawnSrv; + + spawnSrv.request.x = 5; + spawnSrv.request.y = 5; + spawnSrv.request.theta = 0; + spawnSrv.request.name = "stationary_turtle"; + + if(spawnClient.call(spawnSrv)) { + ROS_INFO_STREAM(spawnSrv.response); + } else { + ROS_INFO_STREAM("Failed to spawn turtle"); + } + + spawnSrv.request.x = 25; + spawnSrv.request.y = 10; + spawnSrv.request.theta = 0; + spawnSrv.request.name = "moving_turtle"; + + if(spawnClient.call(spawnSrv)) { + ROS_INFO_STREAM(spawnSrv.response); + } else { + ROS_INFO_STREAM("Failed to spawn turtle"); + } +} +class MovingTurtleReset { + ros::NodeHandle n; +public: + MovingTurtleReset(ros::NodeHandle & newHandle) { + n = newHandle; + ros::ServiceServer service = n.advertiseService("reset_moving_turtle", &MovingTurtleReset::turtleBehaviourServerCallback, this); + ros::spinOnce(); + } + + bool turtleBehaviourServerCallback(software_training_assignment::resetMovingTurtle::Request &req, + software_training_assignment::resetMovingTurtle::Response &res) { + ros::ServiceClient teleportClient = n.serviceClient("moving_turtle/teleport_absolute"); + turtlesim::TeleportAbsolute srv; + srv.request.x = 25; + srv.request.y = 10; + srv.request.theta = 0; + + if(teleportClient.call(srv)) { + return true; + } else { + return false; + } + } +}; + + +class DistancePublisher { + ros::NodeHandle n; + bool endStationarySubscription; + bool endMovingSubscription; + double stationaryTurtleX; + double stationaryTurtleY; + double movingTurtleX; + double movingTurtleY; + + void stationaryCallback(const turtlesim::Pose::ConstPtr& msg) { + stationaryTurtleX = msg->x; + stationaryTurtleY = msg->y; + ROS_INFO_STREAM(stationaryTurtleX); + ROS_INFO_STREAM(stationaryTurtleY); + ROS_INFO_STREAM("Finished getting stationary turtle info"); + endStationarySubscription = true; + } + + void movingCallback(const turtlesim::Pose::ConstPtr& msg) { + movingTurtleX = msg->x; + movingTurtleY = msg->y; + ROS_INFO_STREAM(movingTurtleX); + ROS_INFO_STREAM(movingTurtleY); + ROS_INFO_STREAM("Finished getting moving turtle info"); + endMovingSubscription = true; + } + +public: + DistancePublisher(ros::NodeHandle & newNode) { + n = newNode; + endStationarySubscription = false; + ros::Subscriber stationarySub = n.subscribe("stationary_turtle/pose", 1000, &DistancePublisher::stationaryCallback, this); + while(!endStationarySubscription) { + ros::spinOnce(); + } + endMovingSubscription = false; + ros::Subscriber movingSub = n.subscribe("moving_turtle/pose", 1000, &DistancePublisher::movingCallback, this); + while(!endMovingSubscription) { + ros::spinOnce(); + } + publish(); + } + + void publish() { + ros::Publisher customPub = n.advertise("distanceData", 1000); + // while(ros::ok) { + software_training_assignment::Distance msg; + msg.xDistance = abs(stationaryTurtleX - movingTurtleX); + msg.yDistance = abs(stationaryTurtleY - movingTurtleY); + msg.distance = sqrt(pow(msg.xDistance, 2) + pow(msg.yDistance, 2)); + customPub.publish(msg); + ros::spinOnce(); + // } + } +}; + +class MoveAction { +protected: + ros::NodeHandle nh_; + actionlib::SimpleActionServer as_; + std::string action_name_; + software_training_assignment::MoveFeedback feedback_; + software_training_assignment::MoveResult result_; + bool success_; + bool end_; + float goalX_; + float goalY_; +public: + MoveAction(std::string name) : + as_(nh_, name, boost::bind(&MoveAction::executeCB, this, _1), false), + action_name_(name) + { + as_.start(); + } + + ~MoveAction(void) {} + + void subscriberCB(const turtlesim::Pose::ConstPtr& msg) { + float currentX = msg -> x; + float currentY = msg -> y; + + float velocityX; + if ((currentX - goalX_) > 1) { + velocityX = -1; + } else if ((currentX - goalX_) < -1) { + velocityX = 1; + } else if (std::fabs(currentX - goalX_) >= 0.01) { + velocityX = std::fabs(currentX - goalX_); + } else { + velocityX = 0; + } + + float velocityY; + if ((currentY - goalY_) > 1) { + velocityY = -1; + } else if ((currentY - goalY_) < -1) { + velocityY = 1; + } else if (std::fabs(currentY - goalY_) >= 0.01) { + velocityY = std::fabs(currentY - goalY_); + } else { + velocityY = 0; + } + + if (velocityX == 0 && velocityY == 0) { + end_ = false; + } else { + float distance = sqrt(pow(std::fabs(currentY - goalY_),2) + pow(std::fabs(currentX - goalX_),2)); + feedback_.distance = distance; + as_.publishFeedback(feedback_); + ros::Publisher velocityPub = nh_.advertise("moving_turtle/cmd_vel", 1000); + geometry_msgs::Twist msg; + geometry_msgs::Vector3 linear; + linear.x = velocityX; + linear.y = velocityY; + linear.z = 0; + msg.linear = linear; + + geometry_msgs::Vector3 angular; + angular.x = 0; + angular.y = 0; + angular.z = 0; + msg.angular = angular; + velocityPub.publish(msg); + ros::spinOnce(); + } + + } + void executeCB(const software_training_assignment::MoveGoalConstPtr &goal) { + success_ = true; + end_ = false; + + ROS_INFO_STREAM(goal); + goalX_ = goal -> absolutePos[0]; + goalY_ = goal -> absolutePos[1]; + + ros::Subscriber dataSub = nh_.subscribe("moving_turtle/pose", 1000, &MoveAction::subscriberCB, this); + ros::Time startTime = ros::Time::now(); + while(!end_) { + if(as_.isPreemptRequested() || !ros::ok()) { + ROS_INFO("%s: Preempted", action_name_.c_str()); + // set the action state to preempted + as_.setPreempted(); + success_ = false; + break; + } else { + ros::spinOnce(); + } + } + ros::Time endTime = ros::Time::now(); + ros::Duration duration = endTime - startTime; + if (success_) { + result_.time = duration; + as_.setSucceeded(result_); + } + } +}; +int main(int argc, char **argv) { + ros::init(argc, argv, "turtleBehaviour"); + ros::NodeHandle n; + + clearTurtles(n); + spawnTurtles(n); + + MovingTurtleReset server(n); + + DistancePublisher publisher(n); + + MoveAction action("waypointcd"); + return 0; +} \ No newline at end of file