From f5e0cf94abee4dc14b878f53bd2e4b33be0247a3 Mon Sep 17 00:00:00 2001 From: Urban Pistek Date: Mon, 17 Aug 2020 00:06:00 -0600 Subject: [PATCH 1/8] Launch file created, service clients added --- .gitignore | 1 + .../launch/turtlesim.launch | 11 +++ software_training_assignment/package.xml | 72 +++++++++++++++++++ .../src/my_turtlesim_node.cpp | 45 ++++++++++++ .../srv/resetTurtle.srv | 5 ++ 5 files changed, 134 insertions(+) create mode 100644 .gitignore create mode 100755 software_training_assignment/launch/turtlesim.launch create mode 100644 software_training_assignment/package.xml create mode 100644 software_training_assignment/src/my_turtlesim_node.cpp create mode 100644 software_training_assignment/srv/resetTurtle.srv diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a0f0008 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +CMakeLists.txt diff --git a/software_training_assignment/launch/turtlesim.launch b/software_training_assignment/launch/turtlesim.launch new file mode 100755 index 0000000..00c87a8 --- /dev/null +++ b/software_training_assignment/launch/turtlesim.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ 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..e0d63bf --- /dev/null +++ b/software_training_assignment/package.xml @@ -0,0 +1,72 @@ + + + software_training_assignment + 0.0.0 + The software_training_assignment package + + + + + urbanpistek + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + turtlesim + message_generation + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + turtlesim + message_runtime + + + + + + + + diff --git a/software_training_assignment/src/my_turtlesim_node.cpp b/software_training_assignment/src/my_turtlesim_node.cpp new file mode 100644 index 0000000..966fbe1 --- /dev/null +++ b/software_training_assignment/src/my_turtlesim_node.cpp @@ -0,0 +1,45 @@ +#include + +//Service for turtlesim spawn +#include + +//Service to clear the turtles +//(I am guessing it clears the turtles) +//Based on the wording of the documentation +#include + +int main(int argc, char **argv) { + ros::init(argc, argv, "spawn_turtle"); + ros::NodeHandle nh; + + //Create Client object for the spawn service + ros::ServiceClient spawnClient = nh.ServiceClient ("spawn"); + + //Create the spawn request response objects + turtlesim::Spawn::Request spawn_req; + turtlesim::Spawn::Response spawn_resp; + + //All values to be passed into the service + int x_coords[2] = {5, 25}; + int y_coords[2] = {5, 10}; + string turtlenames[2] = {"stationary_turtle", "moving_turtle"}; + + //Spawns all turtles + for(int idx = 0; 0 < 2; idx++){ + //Fill the request data members + spawn_req.x = x_coords[idx]; + spawn_req.y = y_coords[idx]; + spawn_req.theta = M_PI/2; + spawn_req.name = turtlenames[idx]; + + //Calls the service, waits for the return + bool spawnSuccess = spawnClient.call(spawn_req, spawn_resp); + + //Checks if the service call was successful + if(spawnSuccess) { + ROS_INFO_STREAM("Spawned turtle: " << spawn_resp.name); + } else { + ROS_ERROR_STREAM("Failed to spawn"); + } + } +} \ No newline at end of file diff --git a/software_training_assignment/srv/resetTurtle.srv b/software_training_assignment/srv/resetTurtle.srv new file mode 100644 index 0000000..2979f61 --- /dev/null +++ b/software_training_assignment/srv/resetTurtle.srv @@ -0,0 +1,5 @@ +int64 x # X Coordinate to reset to +int64 y # Y Coordinate to reset to +string name # Name of turtle to be reset +--- +bool success # Response indicating if it was successful \ No newline at end of file From 4b71dcd8683eae1f409e0b85fc9d70d7a2a0d43d Mon Sep 17 00:00:00 2001 From: Urban Pistek Date: Mon, 17 Aug 2020 14:35:29 -0600 Subject: [PATCH 2/8] Added Custom reset turtle service --- .gitignore | 1 + software_training_assignment/CMakeLists.txt | 207 ++++++++++++++++++ .../src/{my_turtlesim_node.cpp => main.cpp} | 7 +- .../src/resetTurtle.cpp | 44 ++++ 4 files changed, 258 insertions(+), 1 deletion(-) create mode 100644 software_training_assignment/CMakeLists.txt rename software_training_assignment/src/{my_turtlesim_node.cpp => main.cpp} (93%) create mode 100644 software_training_assignment/src/resetTurtle.cpp diff --git a/.gitignore b/.gitignore index a0f0008..0394041 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ CMakeLists.txt +!software_training_assignment/CMakeLists.txt \ 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..51f961d --- /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 + rospy + std_msgs + turtlesim + message_generation +) + +## 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 +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( + FILES + resetTurtle.srv +) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_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 rospy std_msgs message_runtime +# 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) diff --git a/software_training_assignment/src/my_turtlesim_node.cpp b/software_training_assignment/src/main.cpp similarity index 93% rename from software_training_assignment/src/my_turtlesim_node.cpp rename to software_training_assignment/src/main.cpp index 966fbe1..c55d9d2 100644 --- a/software_training_assignment/src/my_turtlesim_node.cpp +++ b/software_training_assignment/src/main.cpp @@ -1,4 +1,9 @@ +/* +The main client for the node +*/ + #include +#include //Service for turtlesim spawn #include @@ -9,7 +14,7 @@ #include int main(int argc, char **argv) { - ros::init(argc, argv, "spawn_turtle"); + ros::init(argc, argv, "main"); ros::NodeHandle nh; //Create Client object for the spawn service diff --git a/software_training_assignment/src/resetTurtle.cpp b/software_training_assignment/src/resetTurtle.cpp new file mode 100644 index 0000000..4c620ca --- /dev/null +++ b/software_training_assignment/src/resetTurtle.cpp @@ -0,0 +1,44 @@ +/* +Server for the Reset Turtle Service +*/ + +#include +#include + +//Including the service files for resetTurtle service +#include + +//Struct for the service information +struct turtleInfo { + std::string name; + int x_coord; + int y_coord; +} + +//Defining the service class +class TurtleReset { +public: + TurtleReset() {} + + bool resetTurtlePosition(software_training_assignment::resetTurtleRequest &request, + software_training_assignment::resetTurtleResponse &response){ + ROS_INFO("Turtle Position Reset"); + return true; + }; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "resetTurtle"); + ros::NodeHandle nh; + + //Create an instance of the class + TurtleReset turtle; + + //Link up the service + ros::ServiceServer resetTurtle = nh.advertiseService( + "resetTurtle", &TurtleReset::resetTurtlePosition, &Turtle); + + ros::spin(); //Creates a infinite loop for the server + + return 0; +} \ No newline at end of file From aabc1c4714dbdd77e23bd69182b2233af0826524 Mon Sep 17 00:00:00 2001 From: Urban Pistek Date: Wed, 19 Aug 2020 14:42:45 -0600 Subject: [PATCH 3/8] Added clear turtles service --- software_training_assignment/src/main.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp index c55d9d2..3af4915 100644 --- a/software_training_assignment/src/main.cpp +++ b/software_training_assignment/src/main.cpp @@ -17,6 +17,21 @@ int main(int argc, char **argv) { ros::init(argc, argv, "main"); ros::NodeHandle nh; + // Clear Turtles \\ + + //Create Client object for the Clear service + ros::ServiceClient clearClient = nh.ServiceClient ("clear"); + //Calls the service, waits for the return + bool clearSuccess = clearClient.call(); + //Checks if the service call was successful + if(clearSuccess) { + ROS_INFO_STREAM("Turtles Cleared"); + } else { + ROS_ERROR_STREAM("Failed to clear turtles"); + } + + // Spawn Turtles \\ + //Create Client object for the spawn service ros::ServiceClient spawnClient = nh.ServiceClient ("spawn"); From 249c968b2bea400c3745c4f72b9e51674b3c3f20 Mon Sep 17 00:00:00 2001 From: Urban Pistek Date: Wed, 19 Aug 2020 18:00:41 -0600 Subject: [PATCH 4/8] Custom msg publisher added --- software_training_assignment/CMakeLists.txt | 25 +++++++++++-------- .../msg/turtleDistances.msg | 3 +++ software_training_assignment/package.xml | 2 +- software_training_assignment/src/main.cpp | 22 ++++++++++++++++ 4 files changed, 40 insertions(+), 12 deletions(-) create mode 100644 software_training_assignment/msg/turtleDistances.msg diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt index 51f961d..fba2773 100644 --- a/software_training_assignment/CMakeLists.txt +++ b/software_training_assignment/CMakeLists.txt @@ -49,11 +49,10 @@ find_package(catkin REQUIRED COMPONENTS ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) + add_message_files( + FILES + turtleDistances.msg + ) ## Generate services in the 'srv' folder # add_service_files( @@ -106,7 +105,11 @@ generate_messages( catkin_package( # INCLUDE_DIRS include # LIBRARIES software_training_assignment - CATKIN_DEPENDS roscpp rospy std_msgs message_runtime + CATKIN_DEPENDS + roscpp + rospy + std_msgs + message_runtime # DEPENDS system_lib ) @@ -134,7 +137,7 @@ include_directories( ## 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) + add_executable(${PROJECT_NAME}_node src/main.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -144,12 +147,12 @@ include_directories( ## 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(${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} -# ) + target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ) ############# ## Install ## diff --git a/software_training_assignment/msg/turtleDistances.msg b/software_training_assignment/msg/turtleDistances.msg new file mode 100644 index 0000000..68d3290 --- /dev/null +++ b/software_training_assignment/msg/turtleDistances.msg @@ -0,0 +1,3 @@ +int64 xDistance # distance between x coordinates of the stationary and moving turtles +int64 yDistance # distance between y coordinates of the stationary and moving turtles +int64 distance # distance betweent the two turtles diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml index e0d63bf..d643f93 100644 --- a/software_training_assignment/package.xml +++ b/software_training_assignment/package.xml @@ -58,7 +58,7 @@ rospy std_msgs roscpp - rospy + rospy std_msgs turtlesim message_runtime diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp index 3af4915..b3b3c6b 100644 --- a/software_training_assignment/src/main.cpp +++ b/software_training_assignment/src/main.cpp @@ -4,6 +4,7 @@ The main client for the node #include #include +#include //Service for turtlesim spawn #include @@ -13,6 +14,22 @@ The main client for the node //Based on the wording of the documentation #include +//Including our custom message +#include + +//Class for determining the distances between the turtles +class turtleDistance{ + public: + int xd; + int yd; + int distance; + turtleDistance(int t1_x, int t1_y, int t2_x, int t2_y){ + xd = t1_x - t2_x; + yd = t1_y - t2_y; + distance = sqrt(xd*xd + yd*yd); + } +} + int main(int argc, char **argv) { ros::init(argc, argv, "main"); ros::NodeHandle nh; @@ -62,4 +79,9 @@ int main(int argc, char **argv) { ROS_ERROR_STREAM("Failed to spawn"); } } + + turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); + //Publisher for turtle distances + ros::Publisher turtleDist_pub = nh.advertise("turtle_distnaces", 10); + turtleDist_pub.publish(turtleDist); } \ No newline at end of file From 7a5507a00f268b0617720c223a582f7241f09835 Mon Sep 17 00:00:00 2001 From: Urban Pistek Date: Wed, 19 Aug 2020 18:32:58 -0600 Subject: [PATCH 5/8] Added action client, need to add action server --- software_training_assignment/CMakeLists.txt | 9 ++++----- .../action/moveTurtle.action | 9 +++++++++ software_training_assignment/src/main.cpp | 19 +++++++++++++++++++ 3 files changed, 32 insertions(+), 5 deletions(-) create mode 100644 software_training_assignment/action/moveTurtle.action diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt index fba2773..b825939 100644 --- a/software_training_assignment/CMakeLists.txt +++ b/software_training_assignment/CMakeLists.txt @@ -61,11 +61,10 @@ find_package(catkin REQUIRED COMPONENTS ) ## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) + add_action_files( + FILES + moveTurtle.action + ) ## Generate added messages and services with any dependencies listed here generate_messages( diff --git a/software_training_assignment/action/moveTurtle.action b/software_training_assignment/action/moveTurtle.action new file mode 100644 index 0000000..326ac3f --- /dev/null +++ b/software_training_assignment/action/moveTurtle.action @@ -0,0 +1,9 @@ +# Action goal +int64 abs_x +int64 abs_y +--- +# Action Feedback +int64 abs_distance +--- +# Action result (time in sec) +int64 time \ No newline at end of file diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp index b3b3c6b..1ad9376 100644 --- a/software_training_assignment/src/main.cpp +++ b/software_training_assignment/src/main.cpp @@ -17,6 +17,9 @@ The main client for the node //Including our custom message #include +//Include Action Library +#include + //Class for determining the distances between the turtles class turtleDistance{ public: @@ -84,4 +87,20 @@ int main(int argc, char **argv) { //Publisher for turtle distances ros::Publisher turtleDist_pub = nh.advertise("turtle_distnaces", 10); turtleDist_pub.publish(turtleDist); + + //Creating a Action Client + actionlib::SImpleActionClient ac("move_turtle", true); + ROS_INFO("Waiting for Action Server to start"); + ac.waitForServer(); + int x = 0; + int y = 0; + int goal[2] = {x, y}; + ac.sendGoal(goal); + + bool waitForAction = ac.waitForResult(ros::Duration(30.0)); + if(waitForAction){ + ROS_INFO("Action Finished"); + } else { + ROS_INFO("Action Failed"); + } } \ No newline at end of file From ba63d7b24ec4b14ce73a6de41e0620eb8f9d11d3 Mon Sep 17 00:00:00 2001 From: Urban Pistek Date: Thu, 20 Aug 2020 11:03:59 -0600 Subject: [PATCH 6/8] Adding action implmentation --- .../launch/turtlesim.launch | 2 + software_training_assignment/src/main.cpp | 3 +- .../src/moveTurtle.cpp | 46 +++++++++++++++++++ 3 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 software_training_assignment/src/moveTurtle.cpp diff --git a/software_training_assignment/launch/turtlesim.launch b/software_training_assignment/launch/turtlesim.launch index 00c87a8..6c6292a 100755 --- a/software_training_assignment/launch/turtlesim.launch +++ b/software_training_assignment/launch/turtlesim.launch @@ -6,6 +6,8 @@ + + \ No newline at end of file diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp index 1ad9376..36f3658 100644 --- a/software_training_assignment/src/main.cpp +++ b/software_training_assignment/src/main.cpp @@ -89,9 +89,10 @@ int main(int argc, char **argv) { turtleDist_pub.publish(turtleDist); //Creating a Action Client - actionlib::SImpleActionClient ac("move_turtle", true); + actionlib::SImpleActionClient ac("moveTurtle", true); ROS_INFO("Waiting for Action Server to start"); ac.waitForServer(); + //Define the waypoint for which the turtle should move to int x = 0; int y = 0; int goal[2] = {x, y}; diff --git a/software_training_assignment/src/moveTurtle.cpp b/software_training_assignment/src/moveTurtle.cpp new file mode 100644 index 0000000..8715c24 --- /dev/null +++ b/software_training_assignment/src/moveTurtle.cpp @@ -0,0 +1,46 @@ +/* +Action server for moving the turtle +*/ + +#include +#include +//Subscribe to the geometry_msgs/Twist topic +#include + +bool moveTurtle(int x, int y){ + + + return success; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "moveTurtle"); + ros::NodeHandle nh; + + //Create the action + actionlib::SimpleActionServer as; + std::string action_name = "moveTurtle"; + + //Create messages to be published to feedback and result + actionlib::Feedback feedback; + actionlib::Result result; + + //Get the pointer to the action goal + const actionlib::GoalConstPtr &goal; + //Set the end destination for the action + int x = goal->x; + int y = goal->y; + + //Subscribe to the turtleX/cmd_vel topic + ros::Subscriber sub = nh.subscribe("cmd_vel", 10, moveTurtle); + //Access the message type parameters + geometry_msgs::Twist::vector3::x = x; + + //Create the action server + as(nh, action_name); + as.start(); + + ros::spin(); + + return 0; +} \ No newline at end of file From 17ce17793eb10fc976eecbb5e3177cab03d9d7b8 Mon Sep 17 00:00:00 2001 From: UrbanPistek Date: Tue, 3 Nov 2020 21:09:12 -0500 Subject: [PATCH 7/8] Made small chnages to main and resetTurtle --- .gitignore | 3 ++- software_training_assignment/src/main.cpp | 8 ++++---- software_training_assignment/src/resetTurtle.cpp | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 0394041..b05db73 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ CMakeLists.txt -!software_training_assignment/CMakeLists.txt \ No newline at end of file +!software_training_assignment/CMakeLists.txt +/ros_notes diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp index 36f3658..3676ec3 100644 --- a/software_training_assignment/src/main.cpp +++ b/software_training_assignment/src/main.cpp @@ -23,9 +23,9 @@ The main client for the node //Class for determining the distances between the turtles class turtleDistance{ public: - int xd; - int yd; - int distance; + int xd; + int yd; + int distance; turtleDistance(int t1_x, int t1_y, int t2_x, int t2_y){ xd = t1_x - t2_x; yd = t1_y - t2_y; @@ -89,7 +89,7 @@ int main(int argc, char **argv) { turtleDist_pub.publish(turtleDist); //Creating a Action Client - actionlib::SImpleActionClient ac("moveTurtle", true); + actionlib::SimpleActionClient ac("moveTurtle", true); ROS_INFO("Waiting for Action Server to start"); ac.waitForServer(); //Define the waypoint for which the turtle should move to diff --git a/software_training_assignment/src/resetTurtle.cpp b/software_training_assignment/src/resetTurtle.cpp index 4c620ca..48c1ff1 100644 --- a/software_training_assignment/src/resetTurtle.cpp +++ b/software_training_assignment/src/resetTurtle.cpp @@ -36,7 +36,7 @@ int main(int argc, char **argv) { //Link up the service ros::ServiceServer resetTurtle = nh.advertiseService( - "resetTurtle", &TurtleReset::resetTurtlePosition, &Turtle); + "resetTurtle", &TurtleReset::resetTurtlePosition, &turtle); ros::spin(); //Creates a infinite loop for the server From 18760463396cc7127cab679f28cadc956e338dd2 Mon Sep 17 00:00:00 2001 From: UrbanPistek Date: Sun, 8 Nov 2020 15:02:43 -0500 Subject: [PATCH 8/8] Addressed pull request comments --- software_training_assignment/CMakeLists.txt | 2 +- software_training_assignment/package.xml | 20 ++--- software_training_assignment/src/main.cpp | 44 ++++----- .../src/moveTurtle.cpp | 89 +++++++++++++------ .../src/resetTurtle.cpp | 20 ++--- 5 files changed, 108 insertions(+), 67 deletions(-) diff --git a/software_training_assignment/CMakeLists.txt b/software_training_assignment/CMakeLists.txt index b825939..1f7cbd2 100644 --- a/software_training_assignment/CMakeLists.txt +++ b/software_training_assignment/CMakeLists.txt @@ -55,7 +55,7 @@ find_package(catkin REQUIRED COMPONENTS ) ## Generate services in the 'srv' folder -# add_service_files( + add_service_files( FILES resetTurtle.srv ) diff --git a/software_training_assignment/package.xml b/software_training_assignment/package.xml index d643f93..b792526 100644 --- a/software_training_assignment/package.xml +++ b/software_training_assignment/package.xml @@ -49,19 +49,19 @@ catkin - roscpp - rospy - std_msgs - turtlesim - message_generation + roscpp + rospy + std_msgs + turtlesim + message_generation roscpp rospy std_msgs - roscpp - rospy - std_msgs - turtlesim - message_runtime + roscpp + rospy + std_msgs + turtlesim + message_runtime diff --git a/software_training_assignment/src/main.cpp b/software_training_assignment/src/main.cpp index 3676ec3..cdf8f3f 100644 --- a/software_training_assignment/src/main.cpp +++ b/software_training_assignment/src/main.cpp @@ -83,25 +83,29 @@ int main(int argc, char **argv) { } } - turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); - //Publisher for turtle distances - ros::Publisher turtleDist_pub = nh.advertise("turtle_distnaces", 10); - turtleDist_pub.publish(turtleDist); - - //Creating a Action Client - actionlib::SimpleActionClient ac("moveTurtle", true); - ROS_INFO("Waiting for Action Server to start"); - ac.waitForServer(); - //Define the waypoint for which the turtle should move to - int x = 0; - int y = 0; - int goal[2] = {x, y}; - ac.sendGoal(goal); - - bool waitForAction = ac.waitForResult(ros::Duration(30.0)); - if(waitForAction){ - ROS_INFO("Action Finished"); - } else { - ROS_INFO("Action Failed"); + //Infinite loop to run + while(true){ + turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); + //Publisher for turtle distances + ros::Publisher turtleDist_pub = nh.advertise("turtle_distnaces", 10); + turtleDist_pub.publish(turtleDist); + + //Creating a Action Client + actionlib::SimpleActionClient ac("moveTurtle", true); + ROS_INFO("Waiting for Action Server to start"); + ac.waitForServer(); + //Define the waypoint for which the turtle should move to + int x = 0; + int y = 0; + int goal[2] = {x, y}; + ac.sendGoal(goal); + + bool waitForAction = ac.waitForResult(ros::Duration(30.0)); + if(waitForAction){ + ROS_INFO("Action Finished"); + } else { + ROS_INFO("Action Failed"); + } } + } \ No newline at end of file diff --git a/software_training_assignment/src/moveTurtle.cpp b/software_training_assignment/src/moveTurtle.cpp index 8715c24..090f002 100644 --- a/software_training_assignment/src/moveTurtle.cpp +++ b/software_training_assignment/src/moveTurtle.cpp @@ -4,43 +4,80 @@ Action server for moving the turtle #include #include +#include //Subscribe to the geometry_msgs/Twist topic #include +#include -bool moveTurtle(int x, int y){ - +//Class to define the action server +class MoveTurtleAction { + protected: + ros::NodeHandle nh; + actionlib::SimpleActionServer as; + std::string action_name; + software_training_assignment::moveTurtleFeedback feedback; + software_training_assignment::moveTurtleResult result; - return success; -} + public: -int main(int argc, char **argv) { - ros::init(argc, argv, "moveTurtle"); - ros::NodeHandle nh; + MoveTurtleAction(std::string name) : + as(nh, name, boost::bind(&MoveTurtleAction::executeCB, this, _1), false, action_name(name)) { + as.start(); + } + + ~MoveTurtleAction(void){} + + void executeCB(const software_training_assignment::moveTurtleGoalConstPtr &goal){ + + bool success = false; + geometry_msgs::Twist vel_msg; + ros::Publisher pub = nh.advertise("/turtle1/cmd_vel", 10); + int end_pos_x = goal->abs_x; + int end_pos_y = goal->abs_y; + double distance = sqrt(end_pos_x*end_pos_x + end_pos_y*end_pos_y); + double elapsed_dist = 0; + + //start time + double t0 = ros::Time::now().toSec(); + //move turtle to the end position using the unit vector for the velocity + vel_msg.linear.x = (end_pos_x/distance); + vel_msg.linear.y = (end_pos_y/distance); + vel_msg.linear.z = 0; + vel_msg.angular.x = 0; + vel_msg.angular.y = 0; + vel_msg.angular.z = 0; - //Create the action - actionlib::SimpleActionServer as; - std::string action_name = "moveTurtle"; + //publisher message + pub.publish(vel_msg); + + while(elapsed_dist < distance){ + double t1 = ros::Time::now().toSec(); + double t_diff = t1 - t0; - //Create messages to be published to feedback and result - actionlib::Feedback feedback; - actionlib::Result result; + elapsed_dist = sqrt(vel_msg.linear.x*vel_msg.linear.x + vel_msg.linear.y*vel_msg.linear.y)*t_diff; + feedback.abs_distance = elapsed_dist; + } - //Get the pointer to the action goal - const actionlib::GoalConstPtr &goal; - //Set the end destination for the action - int x = goal->x; - int y = goal->y; + //stop the turtle from moving + vel_msg.linear.x = 0; + vel_msg.linear.y = 0; + pub.publish(vel_msg); - //Subscribe to the turtleX/cmd_vel topic - ros::Subscriber sub = nh.subscribe("cmd_vel", 10, moveTurtle); - //Access the message type parameters - geometry_msgs::Twist::vector3::x = x; + //Get the stop time + double t_end = ros::Time::now().toSec(); - //Create the action server - as(nh, action_name); - as.start(); + //set the result + result.time = (int) (t_end - t0); - ros::spin(); + } +}; + +int main(int argc, char **argv) { + + ros::init(argc, argv, "moveTurtle"); + MoveTurtleAction moveTurtle("moveTurtle"); + ros::spin(); return 0; + } \ No newline at end of file diff --git a/software_training_assignment/src/resetTurtle.cpp b/software_training_assignment/src/resetTurtle.cpp index 48c1ff1..be1840b 100644 --- a/software_training_assignment/src/resetTurtle.cpp +++ b/software_training_assignment/src/resetTurtle.cpp @@ -4,25 +4,26 @@ Server for the Reset Turtle Service #include #include - //Including the service files for resetTurtle service #include -//Struct for the service information -struct turtleInfo { - std::string name; - int x_coord; - int y_coord; -} - //Defining the service class class TurtleReset { public: + int start_x = 25; //Starting x pos for moving turtle + int start_y = 10; //Starting y pos for moving turtle + std::string turtle_name = "moving_turtle"; TurtleReset() {} bool resetTurtlePosition(software_training_assignment::resetTurtleRequest &request, software_training_assignment::resetTurtleResponse &response){ ROS_INFO("Turtle Position Reset"); + + request.x = start_x; + request.y = start_y; + request.name = turtle_name; + response.success = true; + return true; }; } @@ -38,7 +39,6 @@ int main(int argc, char **argv) { ros::ServiceServer resetTurtle = nh.advertiseService( "resetTurtle", &TurtleReset::resetTurtlePosition, &turtle); - ros::spin(); //Creates a infinite loop for the server - + ros::spin(); //Creates a infinite loop to process callbacks return 0; } \ No newline at end of file