From de3ff81ee450eac8b2e09e20c103613ecd72e93a Mon Sep 17 00:00:00 2001 From: ariwasch Date: Wed, 6 Jan 2021 20:27:28 -0500 Subject: [PATCH 1/3] software training --- software_training/.catkin_tools/CATKIN_IGNORE | 0 software_training/.catkin_tools/README | 13 + software_training/.catkin_tools/VERSION | 1 + .../.catkin_tools/profiles/default/build.yaml | 17 ++ .../profiles/default/devel_collisions.txt | 0 .../catkin_tools_prebuild/devel_manifest.txt | 13 + .../catkin_tools_prebuild/package.xml | 10 + software_training/.gitignore | 1 + software_training/CMakeLists.txt | 228 ++++++++++++++++++ software_training/action/action.action | 6 + software_training/msg/Distance.msg | 4 + software_training/msg/Num.msg | 1 + software_training/package.xml | 74 ++++++ software_training/src/action.cpp | 49 ++++ software_training/src/addTurtle.cpp | 58 +++++ software_training/src/listener.cpp | 58 +++++ software_training/src/service.cpp | 46 ++++ software_training/src/talker.cpp | 116 +++++++++ software_training/src/trainingNode.cpp | 91 +++++++ software_training/srv/service.srv | 2 + 20 files changed, 788 insertions(+) create mode 100644 software_training/.catkin_tools/CATKIN_IGNORE create mode 100644 software_training/.catkin_tools/README create mode 100644 software_training/.catkin_tools/VERSION create mode 100644 software_training/.catkin_tools/profiles/default/build.yaml create mode 100644 software_training/.catkin_tools/profiles/default/devel_collisions.txt create mode 100644 software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt create mode 100644 software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml create mode 100644 software_training/.gitignore create mode 100644 software_training/CMakeLists.txt create mode 100644 software_training/action/action.action create mode 100644 software_training/msg/Distance.msg create mode 100644 software_training/msg/Num.msg create mode 100644 software_training/package.xml create mode 100644 software_training/src/action.cpp create mode 100644 software_training/src/addTurtle.cpp create mode 100644 software_training/src/listener.cpp create mode 100644 software_training/src/service.cpp create mode 100644 software_training/src/talker.cpp create mode 100644 software_training/src/trainingNode.cpp create mode 100644 software_training/srv/service.srv diff --git a/software_training/.catkin_tools/CATKIN_IGNORE b/software_training/.catkin_tools/CATKIN_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/software_training/.catkin_tools/README b/software_training/.catkin_tools/README new file mode 100644 index 0000000..4706f47 --- /dev/null +++ b/software_training/.catkin_tools/README @@ -0,0 +1,13 @@ +# Catkin Tools Metadata + +This directory was generated by catkin_tools and it contains persistent +configuration information used by the `catkin` command and its sub-commands. + +Each subdirectory of the `profiles` directory contains a set of persistent +configuration options for separate profiles. The default profile is called +`default`. If another profile is desired, it can be described in the +`profiles.yaml` file in this directory. + +Please see the catkin_tools documentation before editing any files in this +directory. Most actions can be performed with the `catkin` command-line +program. diff --git a/software_training/.catkin_tools/VERSION b/software_training/.catkin_tools/VERSION new file mode 100644 index 0000000..7ceb040 --- /dev/null +++ b/software_training/.catkin_tools/VERSION @@ -0,0 +1 @@ +0.6.1 \ No newline at end of file diff --git a/software_training/.catkin_tools/profiles/default/build.yaml b/software_training/.catkin_tools/profiles/default/build.yaml new file mode 100644 index 0000000..4b2ebed --- /dev/null +++ b/software_training/.catkin_tools/profiles/default/build.yaml @@ -0,0 +1,17 @@ +blacklist: [] +build_space: build +catkin_make_args: [] +cmake_args: [] +devel_layout: linked +devel_space: devel +extend_path: null +install: false +install_space: install +isolate_install: false +jobs_args: [] +log_space: logs +make_args: [] +source_space: src +use_env_cache: false +use_internal_make_jobserver: true +whitelist: [] diff --git a/software_training/.catkin_tools/profiles/default/devel_collisions.txt b/software_training/.catkin_tools/profiles/default/devel_collisions.txt new file mode 100644 index 0000000..e69de29 diff --git a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt new file mode 100644 index 0000000..0d98b05 --- /dev/null +++ b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt @@ -0,0 +1,13 @@ +/home/ariwasch/catkin_ws/src/software_training/build/catkin_tools_prebuild +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.zsh +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.sh /home/ariwasch/catkin_ws/src/software_training/devel/./setup.sh +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/cmake.lock /home/ariwasch/catkin_ws/src/software_training/devel/./cmake.lock +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.zsh /home/ariwasch/catkin_ws/src/software_training/devel/./setup.zsh +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/_setup_util.py /home/ariwasch/catkin_ws/src/software_training/devel/./_setup_util.py +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.bash /home/ariwasch/catkin_ws/src/software_training/devel/./setup.bash +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.bash /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.bash +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/env.sh /home/ariwasch/catkin_ws/src/software_training/devel/./env.sh +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.sh /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.sh +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/ariwasch/catkin_ws/src/software_training/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/ariwasch/catkin_ws/src/software_training/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake +/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/ariwasch/catkin_ws/src/software_training/devel/lib/pkgconfig/catkin_tools_prebuild.pc diff --git a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml new file mode 100644 index 0000000..134c59a --- /dev/null +++ b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml @@ -0,0 +1,10 @@ + + catkin_tools_prebuild + + This package is used to generate catkin setup files. + + 0.0.0 + BSD + jbohren + catkin + diff --git a/software_training/.gitignore b/software_training/.gitignore new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/software_training/.gitignore @@ -0,0 +1 @@ + diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt new file mode 100644 index 0000000..a5ba7c0 --- /dev/null +++ b/software_training/CMakeLists.txt @@ -0,0 +1,228 @@ +cmake_minimum_required(VERSION 3.0.2) +project(software_training) + +## 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 + message_generation + 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 +# Message1.msg +# Message2.msg +# ) +## Generate services in the 'srv' folder +add_service_files( + FILES + service.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 + 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 +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib + CATKIN_DEPENDS + roscpp + std_msgs + message_runtime + actionlib_msgs +) + +########### +## 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.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_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.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(addTurtle src/addTurtle.cpp) +target_link_libraries(addTurtle ${catkin_LIBRARIES}) + +add_executable(talker src/talker.cpp) +target_link_libraries(talker ${catkin_LIBRARIES}) +#add_dependencies(talker software_training_generate_messages_cpp) + +add_executable(listener src/listener.cpp) +target_link_libraries(listener ${catkin_LIBRARIES}) +#add_dependencies(listener software_training_generate_messages_cpp) + +add_executable(service src/service.cpp) +target_link_libraries(service ${catkin_LIBRARIES}) +add_dependencies(service software_training_generate_cpp) + diff --git a/software_training/action/action.action b/software_training/action/action.action new file mode 100644 index 0000000..f499ddf --- /dev/null +++ b/software_training/action/action.action @@ -0,0 +1,6 @@ +float32 x +float32 y +--- +duration time +--- +float32 distance diff --git a/software_training/msg/Distance.msg b/software_training/msg/Distance.msg new file mode 100644 index 0000000..7254481 --- /dev/null +++ b/software_training/msg/Distance.msg @@ -0,0 +1,4 @@ +float32 x +float32 y + +float32 distance diff --git a/software_training/msg/Num.msg b/software_training/msg/Num.msg new file mode 100644 index 0000000..9806981 --- /dev/null +++ b/software_training/msg/Num.msg @@ -0,0 +1 @@ +int64 num diff --git a/software_training/package.xml b/software_training/package.xml new file mode 100644 index 0000000..a4e9faf --- /dev/null +++ b/software_training/package.xml @@ -0,0 +1,74 @@ + + + software_training + 0.0.0 + The software_training package + + + + + ariwasch + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + turtlesim + geometry_msgs + rospy + std_msgs + actionlib_msgs + roscpp + rospy + std_msgs + actionlib_msgs + actionlib_msgs + roscpp + rospy + std_msgs + message_generation + message_runtime + + + + + + + diff --git a/software_training/src/action.cpp b/software_training/src/action.cpp new file mode 100644 index 0000000..3d80be7 --- /dev/null +++ b/software_training/src/action.cpp @@ -0,0 +1,49 @@ +#include +#include +#include +#include "std_msgs/String.h" +#include "geometry_msgs/Twist.h" +#include +#include +#include "software_training/Distance.h" +#include + +int main (int argc, char **argv) +{ + ros::init(argc, argv, "action"); + + ros::NodeHandle nh; + ros::Publisher pub + = nh.advertise("/moving_turtle/cmd_vel", 1000); + // create the action client + // true causes the client to spin its own thread + + actionlib::SimpleActionClient("action", true); + + Server server(n, "action", boost::bind(&execute, _1, &server), false); + server.start(); + + ROS_INFO("Waiting for action server to start."); + // wait for the action server to start + ac.waitForServer(); //will wait for infinite time + + ROS_INFO("Action server started, sending goal."); + // send a goal to the action + actionlib_tutorials::FibonacciGoal goal; + goal.order = 20; + ac.sendGoal(goal); + + //wait for the action to return + bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0)); + + if (finished_before_timeout) + { + actionlib::SimpleClientGoalState state = ac.getState(); + ROS_INFO("Action finished: %s",state.toString().c_str()); + } + else + ROS_INFO("Action did not finish before the time out."); + + //exit + return 0; +} diff --git a/software_training/src/addTurtle.cpp b/software_training/src/addTurtle.cpp new file mode 100644 index 0000000..a542b04 --- /dev/null +++ b/software_training/src/addTurtle.cpp @@ -0,0 +1,58 @@ +//This program spawns a new turtlesim turtle by calling +// the appropriate service. +#include +//The srv class for the service. +#include +#include +#include +#include +int main(int argc, char **argv){ + + ros::init(argc, argv, "addTurtle"); + ros::NodeHandle nh; + +//Create a client object for the spawn service. This +//needs to know the data type of the service and its name. + + ros::ServiceClient spawnClient + = nh.serviceClient("spawn"); + std_srvs::Empty srv; + srv.request; + ros::ServiceClient reset = nh.serviceClient("clear"); + + ros::ServiceClient kill = nh.serviceClient("kill"); + turtlesim::Kill srv2; + srv2.request.name = "turtle1"; + + reset.call(srv); + kill.call(srv2); +//Create the request and response objects. + turtlesim::Spawn::Request req; + turtlesim::Spawn::Response resp; + + req.x = 5; + req.y = 5; + req.theta = M_PI/2; + req.name = "stationary_turtle"; + + turtlesim::Spawn::Request req2; + turtlesim::Spawn::Response resp2; + + req2.x = 25; + req2.y = 10; + req2.theta = M_PI/2; + req2.name = "moving_turtle"; + + ros::service::waitForService("spawn", ros::Duration(5)); + bool success = spawnClient.call(req,resp); + success = spawnClient.call(req2,resp2); + + + if(success){ + ROS_INFO_STREAM("Spawned a turtle named " + << resp.name); + }else{ + ROS_ERROR_STREAM("Failed to spawn."); + } +} + diff --git a/software_training/src/listener.cpp b/software_training/src/listener.cpp new file mode 100644 index 0000000..0c60b11 --- /dev/null +++ b/software_training/src/listener.cpp @@ -0,0 +1,58 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} + +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "listener"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called chatterCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spin(); + + return 0; +} diff --git a/software_training/src/service.cpp b/software_training/src/service.cpp new file mode 100644 index 0000000..dd7054d --- /dev/null +++ b/software_training/src/service.cpp @@ -0,0 +1,46 @@ +//This program spawns a new turtlesim turtle by calling +// the appropriate service. +#include +//The srv class for the service. +#include +#include +#include "turtlesim/TeleportAbsolute.h" + +/*class Service { + +public: + + bool resetPosition(){ + ROS_INFO("RESET"); + return true; + }; + +}*/ + + + bool resetPosition(software_training::ResetTurtle::Request &req, software_training::ResetTurtle::Response &res){ + ROS_INFO("RESET"); + ros::NodeHandle nh; + turtlesim::TeleportAbsolute srv; + srv.request.x = 20 + srv.request.y = 5 + + client = nh.serviceClient("moving_turtle/teleport_absolute"); + if(client.call(srv) == false){ + return false; + } + return true; + + + return true; + }; +int main(int argc, char **argv){ + ros::init(argc, argv, "service"); + ros::NodeHandle nh; + + //Service turtle; + + ros::ServiceServer reset = nh.advertise("service", resetPosition); + + ros::spin(); +} diff --git a/software_training/src/talker.cpp b/software_training/src/talker.cpp new file mode 100644 index 0000000..b7850a5 --- /dev/null +++ b/software_training/src/talker.cpp @@ -0,0 +1,116 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "geometry_msgs/Twist.h" +#include +#include +#include "software_training/Distance.h" + +/** + * This tutorial demonstrates simple sending of messages over the ROS system. + */ + +turtlesim::Pose pos1 +turtlesim::Pose pos2 + +void chatterCallBack(const std_msgs::String::ConstPtr& msg) +{ + pos1.x = msg->x; + pos1.y = msg->y; + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} + +void chatterCallBack2(const std_msgs::String::ConstPtr& msg) +{ + pos2.x = msg->x; + pos2.y = msg->y; + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} + +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "talker"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + //ros::Publisher chatter_pub = n.advertise("chatter", 1000); + ros::Publisher chatter_pub = n.advertise("/moving_turtle/cmd_vel", 1); + + ros::Subscriber turtle1 = n.subscribe("/stationary_turtle/pose",1000, chatterCallBack); + ros::Subscriber turtle2 = n.subscribe("/moving_turtle/pose", 1000, chatterCallBack2); + + + ros::Rate loop_rate(10); + geometry_msgs::Twist move_msg; + + /** + * A count of how many messages we have sent. This is used to create + * a unique string for each message. + */ + int count = 0; + while (ros::ok()) + { + /** + * This is a message object. You stuff it with data, and then publish it. + */ + std_msgs::String msg; + + software_training::Distance msg; + msg.x = pos1.x - pos2.x; + msg.y = pos1.y - pos2.y; + msg.distance = sqrt(pow(msg.x,2) + pow(msg.y, 2)); + std::stringstream ss; + //ss << "hello world " << count; + //msg.data = ss.str(); + + //ROS_INFO("%s", msg.data.c_str()); + + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ + chatter_pub.publish(msg); + + ros::spinOnce(); + + loop_rate.sleep(); + ++count; + } + + + return 0; +} + diff --git a/software_training/src/trainingNode.cpp b/software_training/src/trainingNode.cpp new file mode 100644 index 0000000..c661f71 --- /dev/null +++ b/software_training/src/trainingNode.cpp @@ -0,0 +1,91 @@ +#include "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(); +}; + +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); +} + + +int main(int argc, char **argv) { + ros::init(argc, argv, "trainingNode"); + +} diff --git a/software_training/srv/service.srv b/software_training/srv/service.srv new file mode 100644 index 0000000..410e0f9 --- /dev/null +++ b/software_training/srv/service.srv @@ -0,0 +1,2 @@ +--- +bool success From b8f5f590a73a7e8ecc7fd362936a945a322b9863 Mon Sep 17 00:00:00 2001 From: ariwasch Date: Sun, 10 Jan 2021 00:22:03 -0500 Subject: [PATCH 2/3] fixed? --- software_training/CMakeLists.txt | 48 ++++------------------------- software_training/src/action.cpp | 13 ++++---- software_training/src/addTurtle.cpp | 9 +++++- 3 files changed, 21 insertions(+), 49 deletions(-) diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt index a5ba7c0..db0d42d 100644 --- a/software_training/CMakeLists.txt +++ b/software_training/CMakeLists.txt @@ -1,12 +1,7 @@ cmake_minimum_required(VERSION 3.0.2) project(software_training) -## 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 @@ -16,45 +11,15 @@ find_package(catkin REQUIRED COMPONENTS 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 # Message1.msg # Message2.msg # ) + ## Generate services in the 'srv' folder add_service_files( FILES @@ -62,11 +27,10 @@ add_service_files( ) ## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) +add_action_files( + FILES + action.action +) ## Generate added messages and services with any dependencies listed here generate_messages( @@ -224,5 +188,5 @@ target_link_libraries(listener ${catkin_LIBRARIES}) add_executable(service src/service.cpp) target_link_libraries(service ${catkin_LIBRARIES}) -add_dependencies(service software_training_generate_cpp) +add_dependencies(service software_training_generate_messages_cpp) diff --git a/software_training/src/action.cpp b/software_training/src/action.cpp index 3d80be7..7c61e1e 100644 --- a/software_training/src/action.cpp +++ b/software_training/src/action.cpp @@ -1,8 +1,8 @@ #include #include -#include -#include "std_msgs/String.h" -#include "geometry_msgs/Twist.h" +#include +#include +#include #include #include #include "software_training/Distance.h" @@ -17,15 +17,16 @@ int main (int argc, char **argv) = nh.advertise("/moving_turtle/cmd_vel", 1000); // create the action client // true causes the client to spin its own thread - - actionlib::SimpleActionClient("action", true); + + //actionlib::SimpleActionClient("action", true); + actionlib::SimpleActionClient actionClient("action", true); Server server(n, "action", boost::bind(&execute, _1, &server), false); server.start(); ROS_INFO("Waiting for action server to start."); // wait for the action server to start - ac.waitForServer(); //will wait for infinite time + actionClient.waitForServer(); //will wait for infinite time ROS_INFO("Action server started, sending goal."); // send a goal to the action diff --git a/software_training/src/addTurtle.cpp b/software_training/src/addTurtle.cpp index a542b04..dd0045e 100644 --- a/software_training/src/addTurtle.cpp +++ b/software_training/src/addTurtle.cpp @@ -45,7 +45,7 @@ int main(int argc, char **argv){ ros::service::waitForService("spawn", ros::Duration(5)); bool success = spawnClient.call(req,resp); - success = spawnClient.call(req2,resp2); + bool success2 = spawnClient.call(req2,resp2); if(success){ @@ -53,6 +53,13 @@ int main(int argc, char **argv){ << resp.name); }else{ ROS_ERROR_STREAM("Failed to spawn."); + } + + if(success2){ + ROS_INFO_STREAM("Spawned a turtle named " + << resp2.name); + }else{ + ROS_ERROR_STREAM("Failed to spawn."); } } From e3002dc3484faa82bc59f6d7612ae511a158b8e0 Mon Sep 17 00:00:00 2001 From: Ari Wasch Date: Thu, 8 Jul 2021 21:50:36 -0400 Subject: [PATCH 3/3] I might know ros2?? --- CMakeLists.txt | 188 +++++++++++++++++ action/Software.action | 8 + include/ros-tut/action.hpp | 57 ++++++ include/ros-tut/circle.hpp | 35 ++++ include/ros-tut/publisher.hpp | 34 ++++ include/ros-tut/reset.hpp | 37 ++++ include/ros-tut/spawn.hpp | 43 ++++ include/visibility.hpp | 65 ++++++ msg/Distance.msg | 3 + msg/Num.msg | 1 + package.xml | 40 ++++ software_training/.catkin_tools/CATKIN_IGNORE | 0 software_training/.catkin_tools/README | 13 -- software_training/.catkin_tools/VERSION | 1 - .../.catkin_tools/profiles/default/build.yaml | 17 -- .../profiles/default/devel_collisions.txt | 0 .../catkin_tools_prebuild/devel_manifest.txt | 13 -- .../catkin_tools_prebuild/package.xml | 10 - software_training/.gitignore | 1 - software_training/CMakeLists.txt | 192 ------------------ software_training/action/action.action | 6 - software_training/msg/Distance.msg | 4 - software_training/msg/Num.msg | 1 - software_training/package.xml | 74 ------- software_training/src/action.cpp | 50 ----- software_training/src/addTurtle.cpp | 65 ------ software_training/src/listener.cpp | 58 ------ software_training/src/service.cpp | 46 ----- software_training/src/talker.cpp | 116 ----------- software_training/src/trainingNode.cpp | 91 --------- software_training/srv/service.srv | 2 - src/action.cpp | 137 +++++++++++++ src/add_two_ints_client.cpp | 47 +++++ src/add_two_ints_server.cpp | 28 +++ src/circle.cpp | 23 +++ src/clear_turtles.cpp | 75 +++++++ src/my_node.cpp | 10 + src/my_publisher.cpp | 44 ++++ src/my_subscriber.cpp | 31 +++ src/publisher.cpp | 69 +++++++ src/reset.cpp | 71 +++++++ src/spawn.cpp | 56 +++++ srv/AddThreeInts.srv | 5 + srv/AddTwoInts.srv | 4 + srv/Success.srv | 2 + 45 files changed, 1113 insertions(+), 760 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 action/Software.action create mode 100644 include/ros-tut/action.hpp create mode 100644 include/ros-tut/circle.hpp create mode 100644 include/ros-tut/publisher.hpp create mode 100644 include/ros-tut/reset.hpp create mode 100644 include/ros-tut/spawn.hpp create mode 100644 include/visibility.hpp create mode 100644 msg/Distance.msg create mode 100644 msg/Num.msg create mode 100644 package.xml delete mode 100644 software_training/.catkin_tools/CATKIN_IGNORE delete mode 100644 software_training/.catkin_tools/README delete mode 100644 software_training/.catkin_tools/VERSION delete mode 100644 software_training/.catkin_tools/profiles/default/build.yaml delete mode 100644 software_training/.catkin_tools/profiles/default/devel_collisions.txt delete mode 100644 software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt delete mode 100644 software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml delete mode 100644 software_training/.gitignore delete mode 100644 software_training/CMakeLists.txt delete mode 100644 software_training/action/action.action delete mode 100644 software_training/msg/Distance.msg delete mode 100644 software_training/msg/Num.msg delete mode 100644 software_training/package.xml delete mode 100644 software_training/src/action.cpp delete mode 100644 software_training/src/addTurtle.cpp delete mode 100644 software_training/src/listener.cpp delete mode 100644 software_training/src/service.cpp delete mode 100644 software_training/src/talker.cpp delete mode 100644 software_training/src/trainingNode.cpp delete mode 100644 software_training/srv/service.srv create mode 100644 src/action.cpp create mode 100644 src/add_two_ints_client.cpp create mode 100644 src/add_two_ints_server.cpp create mode 100644 src/circle.cpp create mode 100644 src/clear_turtles.cpp create mode 100644 src/my_node.cpp create mode 100644 src/my_publisher.cpp create mode 100644 src/my_subscriber.cpp create mode 100644 src/publisher.cpp create mode 100644 src/reset.cpp create mode 100644 src/spawn.cpp create mode 100644 srv/AddThreeInts.srv create mode 100644 srv/AddTwoInts.srv create mode 100644 srv/Success.srv diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f23e785 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,188 @@ +cmake_minimum_required(VERSION 3.5) +project(ros_tut) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(turtlesim REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rcutils REQUIRED) +find_package(rcl REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +include_directories(include) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Num.msg" + "srv/AddThreeInts.srv" + "srv/Success.srv" + "msg/Distance.msg" + "action/Software.action" + DEPENDENCIES std_msgs geometry_msgs builtin_interfaces + ) + +# add_executable(talker src/my_publisher.cpp) +# ament_target_dependencies(talker rclcpp std_msgs) +# add_executable(my_node src/my_node.cpp) +# add_executable(listener src/my_subscriber.cpp) +# ament_target_dependencies(listener rclcpp std_msgs) + + +# add_executable(server src/add_two_ints_server.cpp) +# ament_target_dependencies(server +# rclcpp example_interfaces) +# add_executable(client src/add_two_ints_client.cpp) +# ament_target_dependencies(client +# rclcpp example_interfaces) + +# add_executable(clear src/clear_turtles.cpp) +# ament_target_dependencies(clear +# rclcpp ) + +# target_include_directories(my_node PUBLIC +# $ +# $) + +#add kill_turtles as a plugin +add_library(kill_turtles SHARED + src/clear_turtles.cpp) +target_compile_definitions(kill_turtles + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(kill_turtles + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_msgs") +rclcpp_components_register_nodes(kill_turtles "composition::kill_turtles") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(node_plugins "${node_plugins}compositon::kill_turtles;$\n") + +add_library(circle SHARED + src/circle.cpp) +target_compile_definitions(circle + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(circle + "rclcpp" + "rclcpp_components" + "turtlesim" + "geometry_msgs") +rclcpp_components_register_nodes(circle "composition::circle") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(node_plugins "${node_plugins}compositon::circle;$\n") + +add_library(spawn SHARED + src/spawn.cpp) +target_compile_definitions(spawn + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(spawn + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_msgs") +rclcpp_components_register_nodes(spawn "composition::spawn") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(node_plugins "${node_plugins}compositon::spawn;$\n") + +add_library(reset SHARED + src/reset.cpp) +target_compile_definitions(reset + PRIVATE "COMPOSITION_BUILDING_DLL" + ) +ament_target_dependencies(reset + "rclcpp" + "rclcpp_components" + "turtlesim" + "turtlesim" + "std_msgs") +rosidl_target_interfaces(reset ${PROJECT_NAME} "rosidl_typesupport_cpp") # need for custom srv +rclcpp_components_register_nodes(reset "composition::reset") +set(node_plugins "${node_plugins}composition::reset;$) + +add_library(publisher SHARED + src/publisher.cpp) +target_compile_definitions(publisher + PRIVATE "COMPOSITION_BUILDING_DLL" + ) +ament_target_dependencies(publisher + "rclcpp" + "rclcpp_components" + "turtlesim" + "std_msgs") +rosidl_target_interfaces(publisher ${PROJECT_NAME} "rosidl_typesupport_cpp") # need this for custom messages - idk why tho +rclcpp_components_register_nodes(publisher "composition::publisher") +set(node_plugins "${node_plugins}composition::publisher;$") + +# add_library(action SHARED +# src/action.cpp) +# target_compile_definitions(action +# PRIVATE "COMPOSITION_BUILDING_DLL") +# ament_target_dependencies(action +# "rclcpp" +# "rclcpp_components" +# "turtlesim" +# "rclcpp_action" +# "std_msgs" +# "geometry_msgs") +# rosidl_target_interfaces(action ${PROJECT_NAME} "rosidl_typesupport_cpp") # needed for custom action +# # with this we can exexute the component as a node - ros2 run software_training moving_action_server +# rclcpp_components_register_node(action PLUGIN "composition::action" EXECUTABLE action) +add_library(turtle_action_server SHARED + src/action.cpp) +target_compile_definitions(turtle_action_server + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(turtle_action_server + "rclcpp" + "rclcpp_components" + "turtlesim" + "rclcpp_action" + "std_msgs" + "geometry_msgs") +rosidl_target_interfaces(turtle_action_server ${PROJECT_NAME} "rosidl_typesupport_cpp") # needed for custom action +# with this we can exexute the component as a node - ros2 run software_training moving_action_server +# rclcpp_components_register_node(turtle_action_server PLUGIN "composition::action" EXECUTABLE action) +# rclcpp_components_register_nodes(turtle_action_server "composition::action") +rclcpp_components_register_nodes(turtle_action_server "composition::action") +# this way we can execute the component with - ros2 component standalone software_training composition::turtle_service_request_node +set(turtle_action_server "${node_plugins}compositon::action;$\n") + + + +install(TARGETS + # my_node + # talker + # listener + kill_turtles + circle + spawn + reset + turtle_action_server + publisher + # server + # client + RUNTIME DESTINATION lib + LIBRARY DESTINATION lib + ARCHIVE DESTINATION bin) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/action/Software.action b/action/Software.action new file mode 100644 index 0000000..545f94b --- /dev/null +++ b/action/Software.action @@ -0,0 +1,8 @@ +geometry_msgs/Vector3 linear_pos +geometry_msgs/Vector3 angular_pos +--- +uint64 duration +--- +float32 x_pos +float32 y_pos +float32 theta_pos diff --git a/include/ros-tut/action.hpp b/include/ros-tut/action.hpp new file mode 100644 index 0000000..20d145c --- /dev/null +++ b/include/ros-tut/action.hpp @@ -0,0 +1,57 @@ +#ifndef ACTION_HPP_ +#define ACTION_HPP_ + +// #include + +#include +#include +#include // cmd_vel publisher message +#include +#include +#include // ros2 time header +#include // ros2 action header +#include +#include +#include // header for message to get moving turt position + +namespace composition { + +class action : public rclcpp::Node { + public: + explicit action(const rclcpp::NodeOptions &options); + + // nice to have to prevent lengthy repitive code + using GoalHandleActionServer = rclcpp_action::ServerGoalHandle; + + private: + // action server + rclcpp_action::Server::SharedPtr action_server; + + rclcpp::Publisher::SharedPtr publisher; + + rclcpp::Subscription::SharedPtr subscriber; + + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + + // handle accepetd callback function + void handle_accepted(const std::shared_ptr goal_handle); + + // executioner callback function + void execute(const std::shared_ptr goal_handle); + + // for subscriber + static float x; + static float y; + static float theta; + static float linear_velocity; + static float angular_velocity; + + static constexpr unsigned int QUEUE{10}; +}; + +} // namespace composition + +#endif // MOVING_TURTLE_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/include/ros-tut/circle.hpp b/include/ros-tut/circle.hpp new file mode 100644 index 0000000..d61d9f9 --- /dev/null +++ b/include/ros-tut/circle.hpp @@ -0,0 +1,35 @@ +// #ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +// #define TURTLE_SERVICE_REQUEST_NODE_HPP_ +#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +#define TURTLE_SERVICE_REQUEST_NODE_HPP_ + + + +#include +#include +#include +#include +#include + +#include + +// #include +#include +#include "geometry_msgs/msg/twist.hpp" +namespace composition{ + +class circle : public rclcpp::Node { + +public: + explicit circle(const rclcpp::NodeOptions &options); + +private: + rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; + + void circleBoi(); + +}; +} + +#endif diff --git a/include/ros-tut/publisher.hpp b/include/ros-tut/publisher.hpp new file mode 100644 index 0000000..4a92b91 --- /dev/null +++ b/include/ros-tut/publisher.hpp @@ -0,0 +1,34 @@ +// #include + +#include +#include +#include +#include + +namespace composition { + +class publisher : public rclcpp::Node { + public: + explicit publisher(const rclcpp::NodeOptions &options); + + private: + rclcpp::Publisher::SharedPtr publisher_val; + + rclcpp::Subscription::SharedPtr sitting_turtle; + rclcpp::Subscription::SharedPtr moving_turtle; + + rclcpp::TimerBase::SharedPtr timer; + + rclcpp::CallbackGroup::SharedPtr callbacks; + void publish(); + float x_sitting_turtle; + float y_sitting_turtle; + + float x_moving_turtle; + float y_moving_turtle; + + float distance; + + static const unsigned int QUEUE{10}; +}; +} // namespace composition \ No newline at end of file diff --git a/include/ros-tut/reset.hpp b/include/ros-tut/reset.hpp new file mode 100644 index 0000000..57fdddd --- /dev/null +++ b/include/ros-tut/reset.hpp @@ -0,0 +1,37 @@ +#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +#define TURTLE_SERVICE_REQUEST_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std::placeholders; +using namespace std::chrono_literals; + +// #include "rclcpp/rclcpp.hpp" +// #include "std_msgs/msg/string.hpp" +// #include "ros_tut/srv/add_three_ints.hpp" + +namespace composition { + +class reset : public rclcpp::Node { + public: + // SOFTWARE_TRAINING_PUBLIC //what is this? + explicit reset(const rclcpp::NodeOptions &options); + + private: + rclcpp::Service::SharedPtr service; + rclcpp::TimerBase::SharedPtr timer; + + // all the turtles + rclcpp::Client::SharedPtr client; + // SOFTWARE_TRAINING_LOCAL //what is this? + void reset_service(const std::shared_ptr request, + std::shared_ptr response); +}; +} // namespace composition +#endif \ No newline at end of file diff --git a/include/ros-tut/spawn.hpp b/include/ros-tut/spawn.hpp new file mode 100644 index 0000000..8e52e1a --- /dev/null +++ b/include/ros-tut/spawn.hpp @@ -0,0 +1,43 @@ +#ifndef SPAWN_TURTLE_NODELET_HPP_ +#define SPAWN_TURTLE_NODELET_HPP_ + +// #include +// #include +#include +// #include +#include +#include +#include + +namespace composition { + +class spawn : public rclcpp::Node { + +public: + explicit spawn(const rclcpp::NodeOptions &options); + +private: + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + int NUMBER_OF_TURTLES = 2; + + typedef struct turtle_info { + std::string name; + float x; + float y; + } turtle_info; + + std::vector turtle_descriptions = { + {"stationary_turtle", 5, 5}, + {"moving_turtle", 25, 10} + }; + + std::map turtle_description; + + void spawn_offspring(); + +}; + +} +#endif \ No newline at end of file diff --git a/include/visibility.hpp b/include/visibility.hpp new file mode 100644 index 0000000..907a909 --- /dev/null +++ b/include/visibility.hpp @@ -0,0 +1,65 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SOFTWARE_TRAINING__VISIBILITY_H_ +#define SOFTWARE_TRAINING__VISIBILITY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + +#ifdef __GNUC__ +#define SOFTWARE_TRAINING_EXPORT __attribute__((dllexport)) +#define SOFTWARE_TRAINING_IMPORT __attribute__((dllimport)) +#else +#define SOFTWARE_TRAINING_EXPORT __declspec(dllexport) +#define SOFTWARE_TRAINING_IMPORT __declspec(dllimport) +#endif + +#ifdef SOFTWARE_TRAINING_DLL +#define SOFTWARE_TRAINING_PUBLIC SOFTWARE_TRAINING_EXPORT +#else +#define SOFTWARE_TRAINING_PUBLIC SOFTWARE_TRAINING_IMPORT +#endif + +#define SOFTWARE_TRAINING_PUBLIC_TYPE SOFTWARE_TRAINING_PUBLIC + +#define SOFTWARE_TRAINING_LOCAL + +#else + +#define SOFTWARE_TRAINING_EXPORT __attribute__((visibility("default"))) +#define SOFTWARE_TRAINING_IMPORT + +#if __GNUC__ >= 4 +#define SOFTWARE_TRAINING_PUBLIC __attribute__((visibility("default"))) +#define SOFTWARE_TRAINING_LOCAL __attribute__((visibility("hidden"))) +#else +#define SOFTWARE_TRAINING_PUBLIC +#define SOFTWARE_TRAINING_LOCAL +#endif + +#define SOFTWARE_TRAINING_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // SOFTWARE_TRAINING__VISIBILITY_H_ \ No newline at end of file diff --git a/msg/Distance.msg b/msg/Distance.msg new file mode 100644 index 0000000..9e3c23b --- /dev/null +++ b/msg/Distance.msg @@ -0,0 +1,3 @@ +float64 x_pos +float64 y_pos +float64 distance \ No newline at end of file diff --git a/msg/Num.msg b/msg/Num.msg new file mode 100644 index 0000000..5a7c026 --- /dev/null +++ b/msg/Num.msg @@ -0,0 +1 @@ +int64 num \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f910183 --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + + ros_tut + 0.0.0 + TODO: Package description + ari + TODO: License declaration + + ament_cmake + rclcpp + + ament_lint_auto + ament_lint_common + rosidl_default_generators + rclcpp_components + std_msgs + turtlesim + rcutils + geometry_msgs + builtin_interfaces + rosidl_default_generators + rclcpp_action + + + rclcpp_components + launch_ros + + rcutils + turtlesim + geometry_msgs + rosidl_default_runtime + rclcpp_action + + rosidl_interface_packages + + + ament_cmake + + diff --git a/software_training/.catkin_tools/CATKIN_IGNORE b/software_training/.catkin_tools/CATKIN_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/software_training/.catkin_tools/README b/software_training/.catkin_tools/README deleted file mode 100644 index 4706f47..0000000 --- a/software_training/.catkin_tools/README +++ /dev/null @@ -1,13 +0,0 @@ -# Catkin Tools Metadata - -This directory was generated by catkin_tools and it contains persistent -configuration information used by the `catkin` command and its sub-commands. - -Each subdirectory of the `profiles` directory contains a set of persistent -configuration options for separate profiles. The default profile is called -`default`. If another profile is desired, it can be described in the -`profiles.yaml` file in this directory. - -Please see the catkin_tools documentation before editing any files in this -directory. Most actions can be performed with the `catkin` command-line -program. diff --git a/software_training/.catkin_tools/VERSION b/software_training/.catkin_tools/VERSION deleted file mode 100644 index 7ceb040..0000000 --- a/software_training/.catkin_tools/VERSION +++ /dev/null @@ -1 +0,0 @@ -0.6.1 \ No newline at end of file diff --git a/software_training/.catkin_tools/profiles/default/build.yaml b/software_training/.catkin_tools/profiles/default/build.yaml deleted file mode 100644 index 4b2ebed..0000000 --- a/software_training/.catkin_tools/profiles/default/build.yaml +++ /dev/null @@ -1,17 +0,0 @@ -blacklist: [] -build_space: build -catkin_make_args: [] -cmake_args: [] -devel_layout: linked -devel_space: devel -extend_path: null -install: false -install_space: install -isolate_install: false -jobs_args: [] -log_space: logs -make_args: [] -source_space: src -use_env_cache: false -use_internal_make_jobserver: true -whitelist: [] diff --git a/software_training/.catkin_tools/profiles/default/devel_collisions.txt b/software_training/.catkin_tools/profiles/default/devel_collisions.txt deleted file mode 100644 index e69de29..0000000 diff --git a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt deleted file mode 100644 index 0d98b05..0000000 --- a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt +++ /dev/null @@ -1,13 +0,0 @@ -/home/ariwasch/catkin_ws/src/software_training/build/catkin_tools_prebuild -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.zsh -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.sh /home/ariwasch/catkin_ws/src/software_training/devel/./setup.sh -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/cmake.lock /home/ariwasch/catkin_ws/src/software_training/devel/./cmake.lock -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.zsh /home/ariwasch/catkin_ws/src/software_training/devel/./setup.zsh -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/_setup_util.py /home/ariwasch/catkin_ws/src/software_training/devel/./_setup_util.py -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/setup.bash /home/ariwasch/catkin_ws/src/software_training/devel/./setup.bash -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.bash /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.bash -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/env.sh /home/ariwasch/catkin_ws/src/software_training/devel/./env.sh -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/local_setup.sh /home/ariwasch/catkin_ws/src/software_training/devel/./local_setup.sh -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/ariwasch/catkin_ws/src/software_training/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/ariwasch/catkin_ws/src/software_training/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake -/home/ariwasch/catkin_ws/src/software_training/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/ariwasch/catkin_ws/src/software_training/devel/lib/pkgconfig/catkin_tools_prebuild.pc diff --git a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml b/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml deleted file mode 100644 index 134c59a..0000000 --- a/software_training/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml +++ /dev/null @@ -1,10 +0,0 @@ - - catkin_tools_prebuild - - This package is used to generate catkin setup files. - - 0.0.0 - BSD - jbohren - catkin - diff --git a/software_training/.gitignore b/software_training/.gitignore deleted file mode 100644 index 8b13789..0000000 --- a/software_training/.gitignore +++ /dev/null @@ -1 +0,0 @@ - diff --git a/software_training/CMakeLists.txt b/software_training/CMakeLists.txt deleted file mode 100644 index db0d42d..0000000 --- a/software_training/CMakeLists.txt +++ /dev/null @@ -1,192 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(software_training) - - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - message_generation - actionlib_msgs - actionlib -) - - - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -add_service_files( - FILES - service.srv -) - -## Generate actions in the 'action' folder -add_action_files( - FILES - action.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 -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib - CATKIN_DEPENDS - roscpp - std_msgs - message_runtime - actionlib_msgs -) - -########### -## 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.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_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.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(addTurtle src/addTurtle.cpp) -target_link_libraries(addTurtle ${catkin_LIBRARIES}) - -add_executable(talker src/talker.cpp) -target_link_libraries(talker ${catkin_LIBRARIES}) -#add_dependencies(talker software_training_generate_messages_cpp) - -add_executable(listener src/listener.cpp) -target_link_libraries(listener ${catkin_LIBRARIES}) -#add_dependencies(listener software_training_generate_messages_cpp) - -add_executable(service src/service.cpp) -target_link_libraries(service ${catkin_LIBRARIES}) -add_dependencies(service software_training_generate_messages_cpp) - diff --git a/software_training/action/action.action b/software_training/action/action.action deleted file mode 100644 index f499ddf..0000000 --- a/software_training/action/action.action +++ /dev/null @@ -1,6 +0,0 @@ -float32 x -float32 y ---- -duration time ---- -float32 distance diff --git a/software_training/msg/Distance.msg b/software_training/msg/Distance.msg deleted file mode 100644 index 7254481..0000000 --- a/software_training/msg/Distance.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 x -float32 y - -float32 distance diff --git a/software_training/msg/Num.msg b/software_training/msg/Num.msg deleted file mode 100644 index 9806981..0000000 --- a/software_training/msg/Num.msg +++ /dev/null @@ -1 +0,0 @@ -int64 num diff --git a/software_training/package.xml b/software_training/package.xml deleted file mode 100644 index a4e9faf..0000000 --- a/software_training/package.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - software_training - 0.0.0 - The software_training package - - - - - ariwasch - - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - turtlesim - geometry_msgs - rospy - std_msgs - actionlib_msgs - roscpp - rospy - std_msgs - actionlib_msgs - actionlib_msgs - roscpp - rospy - std_msgs - message_generation - message_runtime - - - - - - - diff --git a/software_training/src/action.cpp b/software_training/src/action.cpp deleted file mode 100644 index 7c61e1e..0000000 --- a/software_training/src/action.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "software_training/Distance.h" -#include - -int main (int argc, char **argv) -{ - ros::init(argc, argv, "action"); - - ros::NodeHandle nh; - ros::Publisher pub - = nh.advertise("/moving_turtle/cmd_vel", 1000); - // create the action client - // true causes the client to spin its own thread - - //actionlib::SimpleActionClient("action", true); - actionlib::SimpleActionClient actionClient("action", true); - - Server server(n, "action", boost::bind(&execute, _1, &server), false); - server.start(); - - ROS_INFO("Waiting for action server to start."); - // wait for the action server to start - actionClient.waitForServer(); //will wait for infinite time - - ROS_INFO("Action server started, sending goal."); - // send a goal to the action - actionlib_tutorials::FibonacciGoal goal; - goal.order = 20; - ac.sendGoal(goal); - - //wait for the action to return - bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0)); - - if (finished_before_timeout) - { - actionlib::SimpleClientGoalState state = ac.getState(); - ROS_INFO("Action finished: %s",state.toString().c_str()); - } - else - ROS_INFO("Action did not finish before the time out."); - - //exit - return 0; -} diff --git a/software_training/src/addTurtle.cpp b/software_training/src/addTurtle.cpp deleted file mode 100644 index dd0045e..0000000 --- a/software_training/src/addTurtle.cpp +++ /dev/null @@ -1,65 +0,0 @@ -//This program spawns a new turtlesim turtle by calling -// the appropriate service. -#include -//The srv class for the service. -#include -#include -#include -#include -int main(int argc, char **argv){ - - ros::init(argc, argv, "addTurtle"); - ros::NodeHandle nh; - -//Create a client object for the spawn service. This -//needs to know the data type of the service and its name. - - ros::ServiceClient spawnClient - = nh.serviceClient("spawn"); - std_srvs::Empty srv; - srv.request; - ros::ServiceClient reset = nh.serviceClient("clear"); - - ros::ServiceClient kill = nh.serviceClient("kill"); - turtlesim::Kill srv2; - srv2.request.name = "turtle1"; - - reset.call(srv); - kill.call(srv2); -//Create the request and response objects. - turtlesim::Spawn::Request req; - turtlesim::Spawn::Response resp; - - req.x = 5; - req.y = 5; - req.theta = M_PI/2; - req.name = "stationary_turtle"; - - turtlesim::Spawn::Request req2; - turtlesim::Spawn::Response resp2; - - req2.x = 25; - req2.y = 10; - req2.theta = M_PI/2; - req2.name = "moving_turtle"; - - ros::service::waitForService("spawn", ros::Duration(5)); - bool success = spawnClient.call(req,resp); - bool success2 = spawnClient.call(req2,resp2); - - - if(success){ - ROS_INFO_STREAM("Spawned a turtle named " - << resp.name); - }else{ - ROS_ERROR_STREAM("Failed to spawn."); - } - - if(success2){ - ROS_INFO_STREAM("Spawned a turtle named " - << resp2.name); - }else{ - ROS_ERROR_STREAM("Failed to spawn."); - } -} - diff --git a/software_training/src/listener.cpp b/software_training/src/listener.cpp deleted file mode 100644 index 0c60b11..0000000 --- a/software_training/src/listener.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" - -/** - * This tutorial demonstrates simple receipt of messages over the ROS system. - */ -void chatterCallback(const std_msgs::String::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data.c_str()); -} - -int main(int argc, char **argv) -{ - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. - * For programmatic remappings you can use a different version of init() which takes - * remappings directly, but for most command-line programs, passing argc and argv is - * the easiest way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); - - /** - * ros::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). ros::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ - ros::spin(); - - return 0; -} diff --git a/software_training/src/service.cpp b/software_training/src/service.cpp deleted file mode 100644 index dd7054d..0000000 --- a/software_training/src/service.cpp +++ /dev/null @@ -1,46 +0,0 @@ -//This program spawns a new turtlesim turtle by calling -// the appropriate service. -#include -//The srv class for the service. -#include -#include -#include "turtlesim/TeleportAbsolute.h" - -/*class Service { - -public: - - bool resetPosition(){ - ROS_INFO("RESET"); - return true; - }; - -}*/ - - - bool resetPosition(software_training::ResetTurtle::Request &req, software_training::ResetTurtle::Response &res){ - ROS_INFO("RESET"); - ros::NodeHandle nh; - turtlesim::TeleportAbsolute srv; - srv.request.x = 20 - srv.request.y = 5 - - client = nh.serviceClient("moving_turtle/teleport_absolute"); - if(client.call(srv) == false){ - return false; - } - return true; - - - return true; - }; -int main(int argc, char **argv){ - ros::init(argc, argv, "service"); - ros::NodeHandle nh; - - //Service turtle; - - ros::ServiceServer reset = nh.advertise("service", resetPosition); - - ros::spin(); -} diff --git a/software_training/src/talker.cpp b/software_training/src/talker.cpp deleted file mode 100644 index b7850a5..0000000 --- a/software_training/src/talker.cpp +++ /dev/null @@ -1,116 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "geometry_msgs/Twist.h" -#include -#include -#include "software_training/Distance.h" - -/** - * This tutorial demonstrates simple sending of messages over the ROS system. - */ - -turtlesim::Pose pos1 -turtlesim::Pose pos2 - -void chatterCallBack(const std_msgs::String::ConstPtr& msg) -{ - pos1.x = msg->x; - pos1.y = msg->y; - ROS_INFO("I heard: [%s]", msg->data.c_str()); -} - -void chatterCallBack2(const std_msgs::String::ConstPtr& msg) -{ - pos2.x = msg->x; - pos2.y = msg->y; - ROS_INFO("I heard: [%s]", msg->data.c_str()); -} - -int main(int argc, char **argv) -{ - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. - * For programmatic remappings you can use a different version of init() which takes - * remappings directly, but for most command-line programs, passing argc and argv is - * the easiest way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "talker"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; - - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ - //ros::Publisher chatter_pub = n.advertise("chatter", 1000); - ros::Publisher chatter_pub = n.advertise("/moving_turtle/cmd_vel", 1); - - ros::Subscriber turtle1 = n.subscribe("/stationary_turtle/pose",1000, chatterCallBack); - ros::Subscriber turtle2 = n.subscribe("/moving_turtle/pose", 1000, chatterCallBack2); - - - ros::Rate loop_rate(10); - geometry_msgs::Twist move_msg; - - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ - int count = 0; - while (ros::ok()) - { - /** - * This is a message object. You stuff it with data, and then publish it. - */ - std_msgs::String msg; - - software_training::Distance msg; - msg.x = pos1.x - pos2.x; - msg.y = pos1.y - pos2.y; - msg.distance = sqrt(pow(msg.x,2) + pow(msg.y, 2)); - std::stringstream ss; - //ss << "hello world " << count; - //msg.data = ss.str(); - - //ROS_INFO("%s", msg.data.c_str()); - - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ - chatter_pub.publish(msg); - - ros::spinOnce(); - - loop_rate.sleep(); - ++count; - } - - - return 0; -} - diff --git a/software_training/src/trainingNode.cpp b/software_training/src/trainingNode.cpp deleted file mode 100644 index c661f71..0000000 --- a/software_training/src/trainingNode.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "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(); -}; - -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); -} - - -int main(int argc, char **argv) { - ros::init(argc, argv, "trainingNode"); - -} diff --git a/software_training/srv/service.srv b/software_training/srv/service.srv deleted file mode 100644 index 410e0f9..0000000 --- a/software_training/srv/service.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool success diff --git a/src/action.cpp b/src/action.cpp new file mode 100644 index 0000000..7612fec --- /dev/null +++ b/src/action.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + +action::action(const rclcpp::NodeOptions &options) : Node("action", options) { + // create publisher + this->publisher = this->create_publisher("/moving_turtle/cmd_vel", rclcpp::QoS(QUEUE)); + + auto subscriber_callback = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + this->action::x = msg->x; + this->action::y = msg->y; + this->action::theta = msg->theta; + this->action::linear_velocity = msg->linear_velocity; + this->action::angular_velocity = msg->angular_velocity; + }; + + this->subscriber = this->create_subscription("/moving_turtle/pose", QUEUE, subscriber_callback); + + this->action_server = rclcpp_action::create_server( + this, "action", std::bind(&action::handle_goal, this, _1, _2), std::bind(&action::handle_cancel, this, _1), + std::bind(&action::handle_accepted, this, _1) + + ); +} + +rclcpp_action::GoalResponse action::handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + (void)uuid; + RCLCPP_INFO(this->get_logger(), "GOAL RECIIEIIVEIIVEID"); + RCLCPP_INFO(this->get_logger(), "linear X:%f Y:%f Z:%f", goal->linear_pos.x, goal->linear_pos.y, goal->linear_pos.z); + RCLCPP_INFO(this->get_logger(), "angular X:%f Y:%f Z:%f", goal->angular_pos.x, goal->angular_pos.y, + goal->angular_pos.z); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse action::handle_cancel(const std::shared_ptr goal_handle) { + (void)goal_handle; // not needed + + RCLCPP_INFO(this->get_logger(), "Cancelling goal :("); + + return rclcpp_action::CancelResponse::ACCEPT; +} + +void action::handle_accepted(const std::shared_ptr goal_handle) { + std::thread{std::bind(&action::execute, this, _1), goal_handle}.detach(); +} + +void action::execute(const std::shared_ptr goal_handle) { + rclcpp::Time start_time = this->now(); // get the current time + + RCLCPP_INFO(this->get_logger(), "Excuting Goal"); + + rclcpp::Rate cycle_rate{1}; // set up frequency for goal execution + + const auto goal = goal_handle->get_goal(); + + std::unique_ptr feedback = + std::make_unique(); + + std::unique_ptr result = std::make_unique(); + + float &curr_x = feedback->x_pos; + float &curr_y = feedback->y_pos; + float &curr_theta = feedback->theta_pos; + + float lin_x{0}; + float lin_y{0}; + float lin_z{0}; + + float ang_x{0}; + float ang_y{0}; + float ang_z{0}; + + while (rclcpp::ok() && (lin_x < goal->linear_pos.x || lin_y < goal->linear_pos.y || lin_z < goal->linear_pos.z || + ang_x < goal->angular_pos.x || ang_y < goal->angular_pos.y || ang_z < goal->angular_pos.z)) { + if (goal_handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal Canceled"); + + rclcpp::Time curr_time = this->now(); + rclcpp::Duration time = curr_time - start_time; + long int duration{time.nanoseconds()}; + result->duration = duration; + + goal_handle->canceled(std::move(result)); + return; + } + + auto message_cmd_vel = std::make_unique(); + + message_cmd_vel->linear.x = (lin_x < goal->linear_pos.x) ? lin_x++ : lin_x; + message_cmd_vel->linear.y = (lin_y < goal->linear_pos.y) ? lin_y++ : lin_y; + message_cmd_vel->linear.z = (lin_z < goal->linear_pos.z) ? lin_z++ : lin_z; + message_cmd_vel->angular.x = (ang_x < goal->angular_pos.x) ? ang_x++ : ang_x; + message_cmd_vel->angular.y = (ang_y < goal->angular_pos.y) ? ang_y++ : ang_y; + message_cmd_vel->angular.z = (ang_z < goal->angular_pos.z) ? ang_z++ : ang_z; + + this->publisher->publish(std::move(message_cmd_vel)); + + // now compute feedback + curr_x = this->action::x - lin_x; + curr_y = this->action::y - lin_y; + float theta{0}; + float x1{lin_x}, x2{lin_y}, x3{lin_z}; + float magnitude{static_cast(sqrt((x1 * x1) + (x2 * x2) + (x3 * x3)))}; + theta = acos(x3 / magnitude); + + curr_theta = this->action::theta - theta; + + goal_handle->publish_feedback(std::move(feedback)); + + cycle_rate.sleep(); + } + + // if goal is done + if (rclcpp::ok()) { + rclcpp::Time end = this->now(); + rclcpp::Duration duration = end - start_time; + long int res_time{duration.nanoseconds()}; + result->duration = res_time; + // goal_handle->succeed(std::move(result)); + RCLCPP_INFO(this->get_logger(), "Finish Executing Goal"); + } +} + +} // namespace composition + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::action) diff --git a/src/add_two_ints_client.cpp b/src/add_two_ints_client.cpp new file mode 100644 index 0000000..0f66ede --- /dev/null +++ b/src/add_two_ints_client.cpp @@ -0,0 +1,47 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if (argc != 3) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y"); + return 1; + } + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_client"); + rclcpp::Client::SharedPtr client = + node->create_client("add_two_ints"); + + auto request = std::make_shared(); + request->a = atoll(argv[1]); + request->b = atoll(argv[2]); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints"); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/src/add_two_ints_server.cpp b/src/add_two_ints_server.cpp new file mode 100644 index 0000000..9371546 --- /dev/null +++ b/src/add_two_ints_server.cpp @@ -0,0 +1,28 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include + +void add(const std::shared_ptr request, + std::shared_ptr response) +{ + response->sum = request->a + request->b; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld", + request->a, request->b); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + rclcpp::Service::SharedPtr service = + node->create_service("add_two_ints", &add); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/src/circle.cpp b/src/circle.cpp new file mode 100644 index 0000000..9662f60 --- /dev/null +++ b/src/circle.cpp @@ -0,0 +1,23 @@ +#include +using namespace std::chrono_literals; + + +namespace composition { + + circle::circle(const rclcpp::NodeOptions &options) : Node("circle", options) { + publisher = create_publisher("/turtle1/cmd_vel", 10); + timer = create_wall_timer(2s, std::bind(&circle::circleBoi, this)); + } + + void circle::circleBoi(){ + auto message = geometry_msgs::msg::Twist(); + message.linear.x = 1; + message.angular.z = 1; + publisher->publish(message); + } + +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(composition::circle) + diff --git a/src/clear_turtles.cpp b/src/clear_turtles.cpp new file mode 100644 index 0000000..2ac15eb --- /dev/null +++ b/src/clear_turtles.cpp @@ -0,0 +1,75 @@ +#ifndef TURTLE_SERVICE_REQUEST_NODE_HPP_ +#define TURTLE_SERVICE_REQUEST_NODE_HPP_ + +#include +#include +#include +#include + +#include + +#include + + +using namespace std::placeholders; +using namespace std::chrono_literals; + +// #include "rclcpp/rclcpp.hpp" +// #include "std_msgs/msg/string.hpp" +// #include "ros_tut/srv/add_three_ints.hpp" + +namespace composition { + + +class kill_turtles : public rclcpp::Node { + public: + // SOFTWARE_TRAINING_PUBLIC //what is this? + explicit kill_turtles(const rclcpp::NodeOptions &options); + + private: + rclcpp::Client::SharedPtr client; + rclcpp::TimerBase::SharedPtr timer; + + // all the turtles + std::vector turtle_names = {"turtle1", "moving_turtle", "stationary_turtle"}; + + // SOFTWARE_TRAINING_LOCAL //what is this? + void clear(); +}; + +#endif + + +kill_turtles::kill_turtles(const rclcpp::NodeOptions &options) : Node("kill_turtles", options) { + client = create_client("/kill"); + + timer = create_wall_timer(2s, std::bind(&kill_turtles::clear, this)); // don't understand line +} + +void kill_turtles::clear() { + if (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available after waiting"); + return; + } + + for (std::string &name : turtle_names){ + auto request = std::make_shared(); + request->name = name; + + // using ServiceResponseFuture = kill_turtles::SharedFuture; + auto response_received_callback = [this](rclcpp::Client::SharedFuture response) { + RCLCPP_INFO(this->get_logger(), "POGGERS"); + (void)response; + }; + + auto future_result = client->async_send_request(request, response_received_callback); + } +} +} +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::kill_turtles) //what is this?? diff --git a/src/my_node.cpp b/src/my_node.cpp new file mode 100644 index 0000000..369c19e --- /dev/null +++ b/src/my_node.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world ros-tut package\n"); + return 0; +} diff --git a/src/my_publisher.cpp b/src/my_publisher.cpp new file mode 100644 index 0000000..37fbf93 --- /dev/null +++ b/src/my_publisher.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a +* member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ + public: + MinimalPublisher() + : Node("minimal_publisher"), count_(0) + { + publisher_ = this->create_publisher("topic", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&MinimalPublisher::timer_callback, this)); + } + + private: + void timer_callback() + { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/my_subscriber.cpp b/src/my_subscriber.cpp new file mode 100644 index 0000000..733fcac --- /dev/null +++ b/src/my_subscriber.cpp @@ -0,0 +1,31 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +using std::placeholders::_1; + +class MinimalSubscriber : public rclcpp::Node +{ + public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + subscription_ = this->create_subscription( + "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + } + + private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg) const + { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + } + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/publisher.cpp b/src/publisher.cpp new file mode 100644 index 0000000..8309b94 --- /dev/null +++ b/src/publisher.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +// #include + +#include +#include + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace composition { + +publisher::publisher(const rclcpp::NodeOptions &options) : Node("publisher", options) { + publisher_val = create_publisher("/turtle_distance", 10); + timer = create_wall_timer(1000ms, std::bind(&publisher::publish, this)); + + auto sitting_turtle = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + x_sitting_turtle = msg->x; + y_sitting_turtle = msg->x; + }; + + auto moving_turtle = [this](const turtlesim::msg::Pose::SharedPtr msg) -> void { + x_moving_turtle = msg->x; + y_moving_turtle = msg->y; + }; + + auto callback_option = rclcpp::SubscriptionOptions(); + callback_option.callback_group = callbacks; + + // set ros2 topic statics + callback_option.topic_stats_options.state = + rclcpp::TopicStatisticsState::Enable; + + + this->sitting_turtle = this->create_subscription( + "/stationary_turtle/pose", QUEUE, sitting_turtle, callback_option); + + this->moving_turtle = this->create_subscription( + "/moving_turtle/pose", QUEUE, moving_turtle, callback_option ); +} + +void publisher::publish() { + double pos_y{abs(y_sitting_turtle - y_moving_turtle)}; + double pos_x{abs(x_sitting_turtle - x_sitting_turtle)}; + auto msg = std::make_unique(); + msg->x_pos = pos_x; + msg->y_pos = pos_y; + msg->distance = sqrt((pos_x * pos_x) + (pos_y * pos_y)); + + publisher_val->publish(std::move(msg)); +} +} + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::publisher) \ No newline at end of file diff --git a/src/reset.cpp b/src/reset.cpp new file mode 100644 index 0000000..57d814e --- /dev/null +++ b/src/reset.cpp @@ -0,0 +1,71 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std::placeholders; +using namespace std::chrono_literals; + +namespace composition { + +reset::reset(const rclcpp::NodeOptions &options) : Node("reset", options) { + + client = create_client("/moving_turtle/teleport_absolute"); + + // create service + service = this->create_service( + "/reset", std::bind(&reset::reset_service, this, std::placeholders::_1, std::placeholders::_2)); // what does this do???? + + +} + +void reset::reset_service(const std::shared_ptr request, + std::shared_ptr response) { + + // (void)request; // request is not needed + + RCLCPP_INFO(this->get_logger(), "Starting ..."); + + // make client call to reset turtle + if (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "System Aborted"); + response->result = false; + return; + } + RCLCPP_INFO(this->get_logger(), "Service is not available! Exit!"); + response->result = false; + return; + } + + auto client_request = std::make_shared(); + + // fill request data + client_request->x = 5.4; + client_request->y = 5.4; + client_request->theta = 0; + + // create response callback + auto response_callback = [this](rclcpp::Client::SharedFuture future) -> void { + (void)future; // not needed + RCLCPP_INFO(this->get_logger(), "Turtle Moved!!! YAYAYYAYAYAY"); + }; + + // send client request + auto result = client->async_send_request(client_request, response_callback); + + RCLCPP_INFO(this->get_logger(), "I LIKE TURTLES!!!!!!"); + + response->result = true; +} + +} // namespace composition + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(composition::reset) diff --git a/src/spawn.cpp b/src/spawn.cpp new file mode 100644 index 0000000..e3db920 --- /dev/null +++ b/src/spawn.cpp @@ -0,0 +1,56 @@ +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace composition { + spawn::spawn(const rclcpp::NodeOptions &options) : Node("spawn", options) { + client = this->create_client("/spawn"); + timer = create_wall_timer(2s, std::bind(&spawn::spawn_offspring, this)); + } + + void spawn::spawn_offspring(){ + if (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "Service not available after waiting"); + return; + } + + for (const turtle_info &turt_info : turtle_descriptions) { //whats the point in the & and const here? + + // create request + std::unique_ptr request = + std::make_unique(); + + // fill in repsonse + request->name = turt_info.name; + request->x = turt_info.x; + request->y = turt_info.y; + + // create a callback to call client and because no 'spin()' is available + auto callback = + [this](rclcpp::Client::SharedFuture response) + -> void { + RCLCPP_INFO(this->get_logger(), "Turtle Created: %s", + response.get()->name.c_str()); + rclcpp::shutdown(); + }; + + // send request + auto result = client->async_send_request(std::move(request), callback); + } + + } + +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(composition::spawn) + diff --git a/srv/AddThreeInts.srv b/srv/AddThreeInts.srv new file mode 100644 index 0000000..b2e646f --- /dev/null +++ b/srv/AddThreeInts.srv @@ -0,0 +1,5 @@ +int64 a +int64 b +int64 c +--- +int64 sum diff --git a/srv/AddTwoInts.srv b/srv/AddTwoInts.srv new file mode 100644 index 0000000..3bef723 --- /dev/null +++ b/srv/AddTwoInts.srv @@ -0,0 +1,4 @@ +int64 a +int64 b +--- +int64 sum \ No newline at end of file diff --git a/srv/Success.srv b/srv/Success.srv new file mode 100644 index 0000000..2969a95 --- /dev/null +++ b/srv/Success.srv @@ -0,0 +1,2 @@ +--- +bool result \ No newline at end of file