From 508a0e27ac65d70f48d0ab729c1647181d09722f Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Fri, 10 Jul 2020 18:58:25 -0300 Subject: [PATCH 01/12] Added controler to translate Vector3 goals incoming from the phone to move_base goals. --- docker/create_ros_melodic/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/create_ros_melodic/Dockerfile b/docker/create_ros_melodic/Dockerfile index ca2e1c57..b07cfe78 100644 --- a/docker/create_ros_melodic/Dockerfile +++ b/docker/create_ros_melodic/Dockerfile @@ -39,6 +39,7 @@ RUN pip install --upgrade pip USER $USER RUN echo ". /opt/ros/${ROS1_DISTRO}/setup.bash" >> /home/${USER}/.bashrc +RUN echo ". /create_ws/src/create_autonomy/ca_bringup/scripts/robot_network_config.sh" >> /home/${USER}/.bashrc USER root # Workspace From c72fabde2270d3b26c6128e5aa4292c9612b670a Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Fri, 10 Jul 2020 19:03:21 -0300 Subject: [PATCH 02/12] Added package --- android_assistant/CMakeLists.txt | 207 ++++++++++++++++++++++ android_assistant/launch/send_goal.launch | 6 + android_assistant/package.xml | 81 +++++++++ android_assistant/src/send_robot_goal.cpp | 113 ++++++++++++ 4 files changed, 407 insertions(+) create mode 100644 android_assistant/CMakeLists.txt create mode 100644 android_assistant/launch/send_goal.launch create mode 100644 android_assistant/package.xml create mode 100644 android_assistant/src/send_robot_goal.cpp diff --git a/android_assistant/CMakeLists.txt b/android_assistant/CMakeLists.txt new file mode 100644 index 00000000..123f98e5 --- /dev/null +++ b/android_assistant/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 3.0.2) +project(android_assistant) + +## 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 + actionlib + geometry_msgs + message_generation + roscpp + move_base + move_base_msgs + std_msgs + tf +) + +## 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 +generate_messages( + DEPENDENCIES + geometry_msgs + ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS roscpp std_msgs move_base_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}/android_assistant.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(transfer_goal src/send_robot_goal.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(transfer_goal + ${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 transfer_goal + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_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_android_assistant.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/android_assistant/launch/send_goal.launch b/android_assistant/launch/send_goal.launch new file mode 100644 index 00000000..2b9aa987 --- /dev/null +++ b/android_assistant/launch/send_goal.launch @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/android_assistant/package.xml b/android_assistant/package.xml new file mode 100644 index 00000000..d5e4c021 --- /dev/null +++ b/android_assistant/package.xml @@ -0,0 +1,81 @@ + + + android_assistant + 0.0.0 + The android_assistant package + + + + + create + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + actionlib + roscpp + move_base + move_base_msgs + geometry_msgs + std_msgs + tf + actionlib + roscpp + move_base + move_base_msgs + geometry_msgs + std_msgs + tf + actionlib + geometry_msgs + roscpp + std_msgs + move_base + move_base_msgs + tf + map_server + + + + + + + + diff --git a/android_assistant/src/send_robot_goal.cpp b/android_assistant/src/send_robot_goal.cpp new file mode 100644 index 00000000..f08f3413 --- /dev/null +++ b/android_assistant/src/send_robot_goal.cpp @@ -0,0 +1,113 @@ +#include + +#include +#include +#include +#include +#include "geometry_msgs/Vector3.h" +#include "std_msgs/Float32.h" + +#include +#include +#include +#include + +typedef actionlib::SimpleActionClient + MoveBaseClient; +const std::string NODE_NAME = "navigation_goal"; +bool has_new_goal = false; +bool has_result = true; +geometry_msgs::Vector3 goal; +std_msgs::Float32 distance; + +double inline deg2rad(double deg) { return deg * M_PI / 180.0; }; + +void onNewGoalCallback(const geometry_msgs::Vector3::ConstPtr& msg) { + if (!has_result) { + return; + } + ROS_INFO("New goal received! (x = %.2f, y = %.2f)", msg->x, msg->y); + goal.x = msg->x; + goal.y = msg->y; + goal.z = msg->z; + has_new_goal = true; + has_result = false; +} + +void onResultCallback( + const move_base_msgs::MoveBaseActionResult::ConstPtr& msg) { + has_result = true; + if (msg->status.status == 3) { + ROS_INFO("Robot has arrived to the goal position"); + } else { + ROS_INFO("The base failed for some reason"); + } + ROS_INFO("Waiting for a new goal..."); +} + +void onFeedbackCallback( + const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg) { + float deltaX = goal.x - msg->feedback.base_position.pose.position.x; + float deltaY = goal.y - msg->feedback.base_position.pose.position.y; + distance.data = sqrt(pow(deltaX, 2) + pow(deltaY, 2)); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, NODE_NAME); + ros::NodeHandle n; + MoveBaseClient ac("create1/move_base", true); + + ros::Subscriber sub_goal = + n.subscribe("assistant_goal", 1000, onNewGoalCallback); + ros::Subscriber sub_result = + n.subscribe("create1/move_base/result", 1000, onResultCallback); + ros::Subscriber sub_feedback = + n.subscribe("create1/move_base/feedback", 1000, onFeedbackCallback); + ros::Publisher pub_distance = + n.advertise("assistant_feedback_distance", 1000); + + while (!ac.waitForServer(ros::Duration(5.0))) { + ROS_INFO("Waiting for the move_base action server..."); + } + + // Frecuency of loop in Hz + ros::Rate loop_rate(10); + ROS_INFO("Waiting for a new goal..."); + + while (ros::ok()) { + if (has_new_goal) { + move_base_msgs::MoveBaseGoal move_base_goal; + move_base_goal.target_pose.header.frame_id = "map"; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + try { + move_base_goal.target_pose.pose.position.x = goal.x; + move_base_goal.target_pose.pose.position.y = goal.y; + move_base_goal.target_pose.pose.orientation.w = 1.0; + } catch (int e) { + ROS_WARN_STREAM_NAMED(NODE_NAME, + "Using default 2D pose: [0 m, 0 m, 0 deg]"); + move_base_goal.target_pose.pose.position.x = 0.0; + move_base_goal.target_pose.pose.position.y = 0.0; + } + ROS_INFO("Sending move base goal"); + ac.sendGoal(move_base_goal); + // ac.waitForResult(); + + // if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { + // ROS_INFO("Robot has arrived to the goal position"); + // } else { + // ROS_INFO("The base failed for some reason"); + // } + has_new_goal = false; + // ROS_INFO("Waiting for a new goal..."); + } + + pub_distance.publish(distance); + + // For callbacks + ros::spinOnce(); + // Go to sleep for the configured interval + loop_rate.sleep(); + } + return 0; +} \ No newline at end of file From 28532c62b1ffe2cf92be692638847f6f37800fda Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 03:35:11 -0300 Subject: [PATCH 03/12] Node now published distance to goal and the action result --- android_assistant/src/send_robot_goal.cpp | 45 +++++++++++++---------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/android_assistant/src/send_robot_goal.cpp b/android_assistant/src/send_robot_goal.cpp index f08f3413..cd916b14 100644 --- a/android_assistant/src/send_robot_goal.cpp +++ b/android_assistant/src/send_robot_goal.cpp @@ -6,43 +6,49 @@ #include #include "geometry_msgs/Vector3.h" #include "std_msgs/Float32.h" +#include "std_msgs/String.h" #include #include #include #include +#define STATE_AWAIT_GOAL 0 +#define STATE_PUBLISH_GOAL 1 +#define STATE_GET_RESULT 2 +#define STATE_PUBLISH_RESULT 3 + typedef actionlib::SimpleActionClient MoveBaseClient; const std::string NODE_NAME = "navigation_goal"; -bool has_new_goal = false; -bool has_result = true; + +int node_state = STATE_AWAIT_GOAL; geometry_msgs::Vector3 goal; std_msgs::Float32 distance; - -double inline deg2rad(double deg) { return deg * M_PI / 180.0; }; +std_msgs::String result; void onNewGoalCallback(const geometry_msgs::Vector3::ConstPtr& msg) { - if (!has_result) { + if (node_state != STATE_AWAIT_GOAL) { return; } ROS_INFO("New goal received! (x = %.2f, y = %.2f)", msg->x, msg->y); goal.x = msg->x; goal.y = msg->y; goal.z = msg->z; - has_new_goal = true; - has_result = false; + node_state = STATE_PUBLISH_GOAL; } void onResultCallback( const move_base_msgs::MoveBaseActionResult::ConstPtr& msg) { - has_result = true; if (msg->status.status == 3) { + result.data = "Robot has arrived to the goal position"; ROS_INFO("Robot has arrived to the goal position"); } else { + result.data = "The base failed for some reason"; ROS_INFO("The base failed for some reason"); } ROS_INFO("Waiting for a new goal..."); + node_state = STATE_PUBLISH_RESULT; } void onFeedbackCallback( @@ -64,7 +70,9 @@ int main(int argc, char** argv) { ros::Subscriber sub_feedback = n.subscribe("create1/move_base/feedback", 1000, onFeedbackCallback); ros::Publisher pub_distance = - n.advertise("assistant_feedback_distance", 1000); + n.advertise("assistant_goal_distance", 1000); + ros::Publisher pub_result = + n.advertise("assistant_goal_result", 1000); while (!ac.waitForServer(ros::Duration(5.0))) { ROS_INFO("Waiting for the move_base action server..."); @@ -75,33 +83,30 @@ int main(int argc, char** argv) { ROS_INFO("Waiting for a new goal..."); while (ros::ok()) { - if (has_new_goal) { + if (node_state == STATE_PUBLISH_GOAL) { move_base_msgs::MoveBaseGoal move_base_goal; move_base_goal.target_pose.header.frame_id = "map"; move_base_goal.target_pose.header.stamp = ros::Time::now(); try { move_base_goal.target_pose.pose.position.x = goal.x; move_base_goal.target_pose.pose.position.y = goal.y; - move_base_goal.target_pose.pose.orientation.w = 1.0; } catch (int e) { ROS_WARN_STREAM_NAMED(NODE_NAME, "Using default 2D pose: [0 m, 0 m, 0 deg]"); move_base_goal.target_pose.pose.position.x = 0.0; move_base_goal.target_pose.pose.position.y = 0.0; } + move_base_goal.target_pose.pose.orientation.w = 1.0; ROS_INFO("Sending move base goal"); ac.sendGoal(move_base_goal); - // ac.waitForResult(); - - // if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { - // ROS_INFO("Robot has arrived to the goal position"); - // } else { - // ROS_INFO("The base failed for some reason"); - // } - has_new_goal = false; - // ROS_INFO("Waiting for a new goal..."); + node_state = STATE_GET_RESULT; } + if (node_state == STATE_PUBLISH_RESULT) { + pub_result.publish(result); + node_state = STATE_AWAIT_GOAL; + } + // Publish the distance to the goal. pub_distance.publish(distance); // For callbacks From ff4d4f2279e89db107fd823d4c0f08bc146647b4 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 14:38:58 -0300 Subject: [PATCH 04/12] Added stop command. --- android_assistant/src/send_robot_goal.cpp | 83 ++++++++++++++++------- 1 file changed, 57 insertions(+), 26 deletions(-) diff --git a/android_assistant/src/send_robot_goal.cpp b/android_assistant/src/send_robot_goal.cpp index cd916b14..f55a7e95 100644 --- a/android_assistant/src/send_robot_goal.cpp +++ b/android_assistant/src/send_robot_goal.cpp @@ -4,6 +4,7 @@ #include #include #include +#include "actionlib_msgs/GoalID.h" #include "geometry_msgs/Vector3.h" #include "std_msgs/Float32.h" #include "std_msgs/String.h" @@ -17,6 +18,7 @@ #define STATE_PUBLISH_GOAL 1 #define STATE_GET_RESULT 2 #define STATE_PUBLISH_RESULT 3 +#define STATE_CANCEL 4 typedef actionlib::SimpleActionClient MoveBaseClient; @@ -26,6 +28,7 @@ int node_state = STATE_AWAIT_GOAL; geometry_msgs::Vector3 goal; std_msgs::Float32 distance; std_msgs::String result; +actionlib_msgs::GoalID cancel_msg; void onNewGoalCallback(const geometry_msgs::Vector3::ConstPtr& msg) { if (node_state != STATE_AWAIT_GOAL) { @@ -38,14 +41,29 @@ void onNewGoalCallback(const geometry_msgs::Vector3::ConstPtr& msg) { node_state = STATE_PUBLISH_GOAL; } +void onCancelGoalCallback(const std_msgs::Float32::ConstPtr& msg) { + if (msg->data == 0 && node_state == STATE_GET_RESULT) { + node_state = STATE_CANCEL; + } +} + void onResultCallback( const move_base_msgs::MoveBaseActionResult::ConstPtr& msg) { - if (msg->status.status == 3) { - result.data = "Robot has arrived to the goal position"; - ROS_INFO("Robot has arrived to the goal position"); - } else { - result.data = "The base failed for some reason"; - ROS_INFO("The base failed for some reason"); + switch (msg->status.status) { + case 3: { + result.data = "Robot has arrived to the goal position"; + ROS_INFO("Robot has arrived to the goal position"); + break; + } + case 2: { + result.data = "The goal has been canceled."; + ROS_INFO("The goal has been canceled."); + break; + } + default: { + result.data = "The base failed for some reason"; + ROS_INFO("The base failed for some reason"); + } } ROS_INFO("Waiting for a new goal..."); node_state = STATE_PUBLISH_RESULT; @@ -69,10 +87,14 @@ int main(int argc, char** argv) { n.subscribe("create1/move_base/result", 1000, onResultCallback); ros::Subscriber sub_feedback = n.subscribe("create1/move_base/feedback", 1000, onFeedbackCallback); + ros::Subscriber sub_cancel = + n.subscribe("assistant_goal_cancel", 1000, onCancelGoalCallback); ros::Publisher pub_distance = n.advertise("assistant_goal_distance", 1000); ros::Publisher pub_result = n.advertise("assistant_goal_result", 1000); + ros::Publisher pub_cancel = + n.advertise("create1/move_base/cancel", 1000); while (!ac.waitForServer(ros::Duration(5.0))) { ROS_INFO("Waiting for the move_base action server..."); @@ -83,29 +105,38 @@ int main(int argc, char** argv) { ROS_INFO("Waiting for a new goal..."); while (ros::ok()) { - if (node_state == STATE_PUBLISH_GOAL) { - move_base_msgs::MoveBaseGoal move_base_goal; - move_base_goal.target_pose.header.frame_id = "map"; - move_base_goal.target_pose.header.stamp = ros::Time::now(); - try { - move_base_goal.target_pose.pose.position.x = goal.x; - move_base_goal.target_pose.pose.position.y = goal.y; - } catch (int e) { - ROS_WARN_STREAM_NAMED(NODE_NAME, - "Using default 2D pose: [0 m, 0 m, 0 deg]"); - move_base_goal.target_pose.pose.position.x = 0.0; - move_base_goal.target_pose.pose.position.y = 0.0; + switch (node_state) { + case STATE_PUBLISH_GOAL: { + move_base_msgs::MoveBaseGoal move_base_goal; + move_base_goal.target_pose.header.frame_id = "map"; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + try { + move_base_goal.target_pose.pose.position.x = goal.x; + move_base_goal.target_pose.pose.position.y = goal.y; + } catch (int e) { + ROS_WARN_STREAM_NAMED(NODE_NAME, + "Using default 2D pose: [0 m, 0 m, 0 deg]"); + move_base_goal.target_pose.pose.position.x = 0.0; + move_base_goal.target_pose.pose.position.y = 0.0; + } + move_base_goal.target_pose.pose.orientation.w = 1.0; + ROS_INFO("Sending move base goal"); + ac.sendGoal(move_base_goal); + node_state = STATE_GET_RESULT; + break; + } + case STATE_PUBLISH_RESULT: { + pub_result.publish(result); + node_state = STATE_AWAIT_GOAL; + break; + } + case STATE_CANCEL: { + pub_cancel.publish(cancel_msg); + node_state = STATE_AWAIT_GOAL; + break; } - move_base_goal.target_pose.pose.orientation.w = 1.0; - ROS_INFO("Sending move base goal"); - ac.sendGoal(move_base_goal); - node_state = STATE_GET_RESULT; } - if (node_state == STATE_PUBLISH_RESULT) { - pub_result.publish(result); - node_state = STATE_AWAIT_GOAL; - } // Publish the distance to the goal. pub_distance.publish(distance); From 8d285e1ce3380da773337c250e24a28ceebb5a00 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 15:10:00 -0300 Subject: [PATCH 05/12] Node base topic is set by command line argument. --- android_assistant/src/send_robot_goal.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/android_assistant/src/send_robot_goal.cpp b/android_assistant/src/send_robot_goal.cpp index f55a7e95..ce7ba77e 100644 --- a/android_assistant/src/send_robot_goal.cpp +++ b/android_assistant/src/send_robot_goal.cpp @@ -77,22 +77,22 @@ void onFeedbackCallback( } int main(int argc, char** argv) { + std::string base_topic = argv[1]; ros::init(argc, argv, NODE_NAME); ros::NodeHandle n; MoveBaseClient ac("create1/move_base", true); - ros::Subscriber sub_goal = - n.subscribe("assistant_goal", 1000, onNewGoalCallback); + ros::Subscriber sub_goal = n.subscribe(base_topic, 1000, onNewGoalCallback); ros::Subscriber sub_result = n.subscribe("create1/move_base/result", 1000, onResultCallback); ros::Subscriber sub_feedback = n.subscribe("create1/move_base/feedback", 1000, onFeedbackCallback); ros::Subscriber sub_cancel = - n.subscribe("assistant_goal_cancel", 1000, onCancelGoalCallback); + n.subscribe(base_topic + "_cancel", 1000, onCancelGoalCallback); ros::Publisher pub_distance = - n.advertise("assistant_goal_distance", 1000); + n.advertise(base_topic + "_distance", 1000); ros::Publisher pub_result = - n.advertise("assistant_goal_result", 1000); + n.advertise(base_topic + "_result", 1000); ros::Publisher pub_cancel = n.advertise("create1/move_base/cancel", 1000); From 722c041bc0ba7f5df35a5458e27e2ff8d2133ec0 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 21:35:50 -0300 Subject: [PATCH 06/12] Fixed launch file and namespaces --- android_assistant/launch/send_goal.launch | 4 +++- android_assistant/src/send_robot_goal.cpp | 29 +++++++++++++++++++---- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/android_assistant/launch/send_goal.launch b/android_assistant/launch/send_goal.launch index 2b9aa987..02232f83 100644 --- a/android_assistant/launch/send_goal.launch +++ b/android_assistant/launch/send_goal.launch @@ -1,6 +1,8 @@ + + ns="$(arg ns)" output="screen" + args="$(arg base_topic)"/> \ No newline at end of file diff --git a/android_assistant/src/send_robot_goal.cpp b/android_assistant/src/send_robot_goal.cpp index ce7ba77e..abb3e263 100644 --- a/android_assistant/src/send_robot_goal.cpp +++ b/android_assistant/src/send_robot_goal.cpp @@ -30,6 +30,7 @@ std_msgs::Float32 distance; std_msgs::String result; actionlib_msgs::GoalID cancel_msg; +// Called when a new goal is received. void onNewGoalCallback(const geometry_msgs::Vector3::ConstPtr& msg) { if (node_state != STATE_AWAIT_GOAL) { return; @@ -41,12 +42,14 @@ void onNewGoalCallback(const geometry_msgs::Vector3::ConstPtr& msg) { node_state = STATE_PUBLISH_GOAL; } +// Called when a goal is canceled. void onCancelGoalCallback(const std_msgs::Float32::ConstPtr& msg) { if (msg->data == 0 && node_state == STATE_GET_RESULT) { node_state = STATE_CANCEL; } } +// Called when move base has a result. void onResultCallback( const move_base_msgs::MoveBaseActionResult::ConstPtr& msg) { switch (msg->status.status) { @@ -69,6 +72,7 @@ void onResultCallback( node_state = STATE_PUBLISH_RESULT; } +// Called on every feedback message from move base. void onFeedbackCallback( const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg) { float deltaX = goal.x - msg->feedback.base_position.pose.position.x; @@ -80,22 +84,36 @@ int main(int argc, char** argv) { std::string base_topic = argv[1]; ros::init(argc, argv, NODE_NAME); ros::NodeHandle n; - MoveBaseClient ac("create1/move_base", true); + MoveBaseClient ac("move_base", true); + // Subscribes to goal messages. ros::Subscriber sub_goal = n.subscribe(base_topic, 1000, onNewGoalCallback); + + // Subscribes to move base result messages. ros::Subscriber sub_result = - n.subscribe("create1/move_base/result", 1000, onResultCallback); + n.subscribe("move_base/result", 1000, onResultCallback); + + // Subscribes to move base feedback messages. ros::Subscriber sub_feedback = - n.subscribe("create1/move_base/feedback", 1000, onFeedbackCallback); + n.subscribe("move_base/feedback", 1000, onFeedbackCallback); + + // Subscribes to cancel-goal messages. ros::Subscriber sub_cancel = n.subscribe(base_topic + "_cancel", 1000, onCancelGoalCallback); + + // Publishes the remaining distance to the goal in meters. ros::Publisher pub_distance = n.advertise(base_topic + "_distance", 1000); + + // Publishes the action restult. ros::Publisher pub_result = n.advertise(base_topic + "_result", 1000); + + // Publishes cancel-goal messages. ros::Publisher pub_cancel = - n.advertise("create1/move_base/cancel", 1000); + n.advertise("move_base/cancel", 1000); + // Wait for move base action server. while (!ac.waitForServer(ros::Duration(5.0))) { ROS_INFO("Waiting for the move_base action server..."); } @@ -106,6 +124,7 @@ int main(int argc, char** argv) { while (ros::ok()) { switch (node_state) { + // Publishes the goal to the move base server. case STATE_PUBLISH_GOAL: { move_base_msgs::MoveBaseGoal move_base_goal; move_base_goal.target_pose.header.frame_id = "map"; @@ -125,11 +144,13 @@ int main(int argc, char** argv) { node_state = STATE_GET_RESULT; break; } + // Publishes the goal result. case STATE_PUBLISH_RESULT: { pub_result.publish(result); node_state = STATE_AWAIT_GOAL; break; } + // Publishes a cancel message to move base. case STATE_CANCEL: { pub_cancel.publish(cancel_msg); node_state = STATE_AWAIT_GOAL; From c370b5d6f3c4702a30442da46dc1dac281596178 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 22:49:02 -0300 Subject: [PATCH 07/12] Updated README --- README.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index d46b6888..619fe70b 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,17 @@ -# create_autonomy +# create_autonomy + Android Assistant [ROS](http://ros.org) driver for iRobot [Create 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx). This package wraps the C++ library [libcreate][libcreate], which uses iRobot's [Open Interface Specification][oi_spec]. +The "android_asssitant" package has been added to serve as an interface between the Android Apllication to send voice commands and move base. +To run the node in the package to communicate with the app, use: + +``` +roslaunch android_assistant send_goal.launch base_topic:=goal_assistant +``` + +Where "base_topic" should be the same topic set up in the application's Settings page. In this case, "goal_assistant" is the default topic. + * ROS wiki page: http://wiki.ros.org/create_autonomy * Support: [ROS Answers (tag: create_autonomy)](http://answers.ros.org/questions/scope:all/sort:activity-desc/tags:create_autonomy/page:1/) * Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca)) From baeecb1b032a64556172cd83c06904a2a008399c Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 23:09:38 -0300 Subject: [PATCH 08/12] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 619fe70b..9eeaa670 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [ROS](http://ros.org) driver for iRobot [Create 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx). This package wraps the C++ library [libcreate][libcreate], which uses iRobot's [Open Interface Specification][oi_spec]. -The "android_asssitant" package has been added to serve as an interface between the Android Apllication to send voice commands and move base. +The "android_asssitant" package allows to control the robot through an Android application, by the use of voice commands. The node serves as an interface between the app and move base. To run the node in the package to communicate with the app, use: ``` From afde4aa771c536609493b13ae02af24c1f2c1a01 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Sat, 11 Jul 2020 23:10:37 -0300 Subject: [PATCH 09/12] Updated README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 9eeaa670..f5f80ff6 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ [ROS](http://ros.org) driver for iRobot [Create 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx). This package wraps the C++ library [libcreate][libcreate], which uses iRobot's [Open Interface Specification][oi_spec]. -The "android_asssitant" package allows to control the robot through an Android application, by the use of voice commands. The node serves as an interface between the app and move base. +The "android_asssitant" package allows to control the robot through an [Android application](https://github.com/adevitturi/ROS_Voice_Commands_App), by the use of voice commands. The node serves as an interface between the app and move base. To run the node in the package to communicate with the app, use: ``` From add173b078861aa66e0ea3aa6871bf4bdc8c7001 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Mon, 13 Jul 2020 10:07:17 -0300 Subject: [PATCH 10/12] Changed readme --- README.md | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/README.md b/README.md index f5f80ff6..d46b6888 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,8 @@ -# create_autonomy + Android Assistant +# create_autonomy [ROS](http://ros.org) driver for iRobot [Create 2](http://www.irobot.com/About-iRobot/STEM/Create-2.aspx). This package wraps the C++ library [libcreate][libcreate], which uses iRobot's [Open Interface Specification][oi_spec]. -The "android_asssitant" package allows to control the robot through an [Android application](https://github.com/adevitturi/ROS_Voice_Commands_App), by the use of voice commands. The node serves as an interface between the app and move base. -To run the node in the package to communicate with the app, use: - -``` -roslaunch android_assistant send_goal.launch base_topic:=goal_assistant -``` - -Where "base_topic" should be the same topic set up in the application's Settings page. In this case, "goal_assistant" is the default topic. - * ROS wiki page: http://wiki.ros.org/create_autonomy * Support: [ROS Answers (tag: create_autonomy)](http://answers.ros.org/questions/scope:all/sort:activity-desc/tags:create_autonomy/page:1/) * Author: [Jacob Perron](http://jacobperron.ca) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca)) From aa9596c23335a6ba9689ac78fc0e052c96f5d21b Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Mon, 13 Jul 2020 10:11:11 -0300 Subject: [PATCH 11/12] Changed docs/STRUCTURE.md --- docs/STRUCTURE.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/STRUCTURE.md b/docs/STRUCTURE.md index b6102ee6..07ceb423 100644 --- a/docs/STRUCTURE.md +++ b/docs/STRUCTURE.md @@ -74,3 +74,13 @@ a. `ca_imu` IMU driver for [MPU9255](https://store.invensense.com/products/detail/MPU9255-InvenSense-Inc/520231/). + +14. `android_assistant` + The "android_asssitant" package allows to control the robot through an [Android application](https://github.com/adevitturi/ROS_Voice_Commands_App), by the use of voice commands. The node serves as an interface between the app and move base. + To run the node in the package to communicate with the app, use: + + ``` + roslaunch android_assistant send_goal.launch base_topic:=goal_assistant + ``` + + Where "base_topic" should be the same topic set up in the application's Settings page. In this case, "goal_assistant" is the default topic. From 327559443f40710163f2e135f49d5730753a2f09 Mon Sep 17 00:00:00 2001 From: Agustin Devitturi Date: Mon, 13 Jul 2020 10:13:02 -0300 Subject: [PATCH 12/12] Changed docs/STRUCTURE.md --- docs/STRUCTURE.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/STRUCTURE.md b/docs/STRUCTURE.md index 07ceb423..b302e1c8 100644 --- a/docs/STRUCTURE.md +++ b/docs/STRUCTURE.md @@ -76,6 +76,7 @@ IMU driver for [MPU9255](https://store.invensense.com/products/detail/MPU9255-InvenSense-Inc/520231/). 14. `android_assistant` + The "android_asssitant" package allows to control the robot through an [Android application](https://github.com/adevitturi/ROS_Voice_Commands_App), by the use of voice commands. The node serves as an interface between the app and move base. To run the node in the package to communicate with the app, use: