diff --git a/hubero_bringup_gazebo_ros/CMakeLists.txt b/hubero_bringup_gazebo_ros/CMakeLists.txt index bb5e61c9..4530edfd 100644 --- a/hubero_bringup_gazebo_ros/CMakeLists.txt +++ b/hubero_bringup_gazebo_ros/CMakeLists.txt @@ -16,6 +16,13 @@ find_package(catkin REQUIRED COMPONENTS catkin_package() ## Install +install( + PROGRAMS + scripts/trajectory_spawner.sh + scripts/xacro_spawner.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + # ref: https://github.com/pal-robotics/tiago_robot/blob/kinetic-devel/tiago_bringup/CMakeLists.txt foreach(dir launch maps rviz worlds) install(DIRECTORY ${dir}/ diff --git a/hubero_bringup_gazebo_ros/README.md b/hubero_bringup_gazebo_ros/README.md index 7fd04c77..5c54c6b0 100644 --- a/hubero_bringup_gazebo_ros/README.md +++ b/hubero_bringup_gazebo_ros/README.md @@ -4,11 +4,11 @@ Package contains `.launch` files, `.pgm` maps and `.world` files for evaluation ## Prerequisites -Before you try to run exemplary worlds, you need to setup Gazebo simulator with a required set of models (used in provided worlds). Visit [Gazebo setup wiki page](https://github.com/rayvburn/hubero/wiki/Gazebo-setup) and start with setup of `3DGems` model pack (this is required, rest may be optional depending on your system configuration). +Before you try to run exemplary worlds, you need to set up the Gazebo simulator with a required set of models (used in provided worlds). Visit the [Gazebo setup wiki page](https://github.com/rayvburn/hubero/wiki/Gazebo-setup) and start setting up the `3DGems` model pack (this one is required, others may be optional depending on your system configuration). ## Run -Before a launch of an exemplary world one must apply both ROS workspace and Gazebo configurations: +Before the launch of an exemplary world, one must apply both ROS workspace and Gazebo configurations: ```bash source /devel/setup.bash @@ -21,26 +21,34 @@ Then, `parking` world, defined for Gazebo, with all necessary Actor interfaces ( roslaunch hubero_bringup_gazebo_ros example.launch world:=parking rviz:=true ``` -If all went OK, after few seconds after start you should see `move_base` log: +If all went OK, after few seconds after the start you should see `move_base` log: ```console [ INFO] [1643660811.668211218, 8.331000000]: odom received! ``` -**NOTE**: actor names defined in Gazebo `.world` must match names given to `.launch` file. - -## Spawn +## Spawning an actor The crucial question is: how to spawn an `Actor` with a `ActorPlugin` controller in a custom Gazebo world? -Since Gazebo entities are defined in `.world` file, one must add Actor definition to the Gazebo world. Actor definition consists of: +### Spawn with a `spawn_model` service + +A handy `spawn_actor.launch` was prepared to spawn an actor controlled with the `HuBeRo` plugin in the Gazebo. Once the simulation is running, type: + +```sh +roslaunch hubero_bringup_gazebo_ros spawn_actor.launch actor_name:= +``` + +### Spawn directly in the `.world` file + +Typically, Gazebo entities are defined in a `.world` file, so one can also add the `Actor` definition directly to the Gazebo world. Actor definition consists of: - starting pose definition, - animation definitions, - attached sensor relative pose and definition, -- plugin used to control Actor. +- plugin used to control the `Actor`. -Generally, one should use the exemplary worlds definitions as a reference to use actors in their custom worlds. Simply open `living_room.world` or `parking.world` and copy section that is related to a given actor: +Generally, one should use the exemplary worlds definitions as a reference to add actors to their custom worlds. To accomplish that, simply open `living_room.world` or `parking.world` and copy a section that is related to a given actor: ```xml @@ -53,13 +61,20 @@ Generally, one should use the exemplary worlds definitions as a reference to use ``` +**NOTE**: actor names defined in Gazebo `.world` must match names given to the `.launch` file. + ## ROS interface -Actors will not have mobility skills without connection with ROS Navigation stack. Also, Actors will not be able to receive task requests without connection to ROS topics. Use `example.launch` parameter definitions to properly define new Actor interfaces. +Actors will not have mobility skills without the connection with ROS Navigation stack. Also, Actors will not be able to receive task requests without connection to ROS topics. Use `example.launch` parameter definitions to properly define new Actor interfaces. ## Gazebo world map -To create an ideal map of the Gazebo world one may use [`pgm_map_creator`](https://github.com/hyfan1116/pgm_map_creator) package. Examples of use are provided in `maps/*.yaml` files. +To create an ideal map of the Gazebo world one may use `pgm_map_creator`: + +- [`hyfan1116/pgm_map_creator`](https://github.com/hyfan1116/pgm_map_creator) with ROS Kinetic and Ubuntu 16.04 +- [`rayvburn/pgm_map_creator`](https://github.com/rayvburn/pgm_map_creator.git) with ROS Melodic and Ubuntu 18.04 + +Examples of use are provided in `maps/*.yaml` files. One must remember to place/uncomment `collision_map_creator` plugin definition in the `.world` file: @@ -69,13 +84,13 @@ One must remember to place/uncomment `collision_map_creator` plugin definition i ## Debugging -`example.launch` provides a possibility to debug Actor controller plugin. First, user should extend `GAZEBO_PLUGIN_PATH`: +`example.launch` provides a possibility to debug the `Actor` controller plugin. First, the user should extend `GAZEBO_PLUGIN_PATH`: ```bash -export GAZEBO_PLUGIN_PATH=/lib:$GAZEBO_PLUGIN_PATH +export GAZEBO_PLUGIN_PATH=/devel/.private/hubero_gazebo/lib:$GAZEBO_PLUGIN_PATH ``` -Then, run Gazebo world with the required plugins. ROS1 distro should be adjusted by user: +Then, run the Gazebo world with the required plugins. ROS1 distro should be adjusted by the user: ```bash gazebo -s /opt/ros/kinetic/lib/libgazebo_ros_paths_plugin.so -s /opt/ros/kinetic/lib/libgazebo_ros_api_plugin.so /parking.world diff --git a/hubero_bringup_gazebo_ros/launch/example.launch b/hubero_bringup_gazebo_ros/launch/example.launch index 64be94ac..420e018c 100644 --- a/hubero_bringup_gazebo_ros/launch/example.launch +++ b/hubero_bringup_gazebo_ros/launch/example.launch @@ -8,6 +8,9 @@ + + + @@ -25,9 +28,11 @@ - + + + - + diff --git a/hubero_bringup_gazebo_ros/launch/spawn_actor.launch b/hubero_bringup_gazebo_ros/launch/spawn_actor.launch new file mode 100644 index 00000000..6750129a --- /dev/null +++ b/hubero_bringup_gazebo_ros/launch/spawn_actor.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hubero_bringup_gazebo_ros/package.xml b/hubero_bringup_gazebo_ros/package.xml index 0d835018..789433ce 100644 --- a/hubero_bringup_gazebo_ros/package.xml +++ b/hubero_bringup_gazebo_ros/package.xml @@ -1,7 +1,7 @@ hubero_bringup_gazebo_ros - 0.6.0 + 0.7.0 The hubero_bringup_gazebo_ros package Jarosław Karwowski diff --git a/hubero_bringup_gazebo_ros/scripts/trajectory_spawner.sh b/hubero_bringup_gazebo_ros/scripts/trajectory_spawner.sh new file mode 100755 index 00000000..8d9cb403 --- /dev/null +++ b/hubero_bringup_gazebo_ros/scripts/trajectory_spawner.sh @@ -0,0 +1,59 @@ +#!/usr/bin/env bash +# +# This is a script for spawning a non-HuBeRo actor (generic Gazebo actor) with a predefined trajectory to be executed +# +# NOTE: the contents of the input_file (2nd argument) should be in the following form: +# +# +# +# +# +# +# +# +# +# +# +# +# +# walk.dae +# +# +# walk.dae +# true +# +# +# +# +# +# + +# Pass arguments +actor_name="$1" +# xacro/URDF file location +input_file="$2" + +# compose arguments to be passed to the xacro +XACRO_ARGS="actor_name:=${actor_name}" + +# deletes lines containing SCRIPTDELETE phrase +SDF=$(xacro "$input_file" $XACRO_ARGS | sed "/SCRIPTDELETE/d") + +# save sdf to file +SDF_FILE="/tmp/trajectory_spawner_${actor_name}" +echo $SDF > $SDF_FILE + +# spawn coords are defined in the xacro +rosrun gazebo_ros spawn_model -sdf -file $SDF_FILE -model $actor_name diff --git a/hubero_bringup_gazebo_ros/scripts/xacro_spawner.sh b/hubero_bringup_gazebo_ros/scripts/xacro_spawner.sh new file mode 100755 index 00000000..45c3834a --- /dev/null +++ b/hubero_bringup_gazebo_ros/scripts/xacro_spawner.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash +# +# Pass xacro arguments one by one, beware of the order +actor_name="$1" +pose_x="$2" +pose_y="$3" +pose_z="$4" +pose_roll="$5" +pose_pitch="$6" +pose_yaw="$7" +visualize_laser="$8" +skin_file="$9" + +# compose arguments to be passed to the xacro +XACRO_ARGS="actor_name:=${actor_name} pose_x:=${pose_x} pose_y:=${pose_y} pose_z:=${pose_z} pose_roll:=${pose_roll} pose_pitch:=${pose_pitch} pose_yaw:=${pose_yaw} visualize_laser:=${visualize_laser} skin_file:=${skin_file}" + +# deletes lines containing SCRIPTDELETE phrase +SDF=$(xacro "$(rospack find hubero_gazebo)/urdf/actor.xacro" $XACRO_ARGS | sed "/SCRIPTDELETE/d") + +# save sdf to file +SDF_FILE="/tmp/xacro_spawner_${actor_name}" +echo $SDF > $SDF_FILE + +# spawn coords are defined in the xacro +rosrun gazebo_ros spawn_model -sdf -file $SDF_FILE -model $actor_name diff --git a/hubero_bringup_gazebo_ros/worlds/parking.world b/hubero_bringup_gazebo_ros/worlds/parking.world index a3394215..1434e726 100644 --- a/hubero_bringup_gazebo_ros/worlds/parking.world +++ b/hubero_bringup_gazebo_ros/worlds/parking.world @@ -3910,12 +3910,6 @@ true - - stand_up.dae - 1.000000 - true - - run.dae 1.000000 @@ -4019,12 +4013,6 @@ true - - stand_up.dae - 1.000000 - true - - run.dae 1.000000 diff --git a/hubero_gazebo/urdf/actor.xacro b/hubero_gazebo/urdf/actor.xacro new file mode 100644 index 00000000..9165ab2b --- /dev/null +++ b/hubero_gazebo/urdf/actor.xacro @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw} + + ${skin_file} + 1.0 + + + + + + 0 -0.8 0.55 0 -1.57 -1.57 + ${visualize_laser} + 10 + + + + 720 + 1 + -2.170796 + 2.170796 + + + + 0.10 + 10.0 + 0.01 + + + gaussian + + 0.0 + 0.01 + + + + /hubero/${actor_name}/receptor/laser_scan + + base_laser_link + + + + + + walk.dae + 1.000000 + true + + + stand.dae + 1.000000 + true + + + sit_down.dae + 1.000000 + true + + + sitting.dae + 1.000000 + true + + + stand_up.dae + 1.000000 + true + + + run.dae + 1.000000 + true + + + talk_a.dae + 1.000000 + true + + + talk_b.dae + 1.000000 + true + + + + + + + diff --git a/hubero_interfaces/include/hubero_interfaces/navigation_base.h b/hubero_interfaces/include/hubero_interfaces/navigation_base.h index 94ddf298..519822cc 100644 --- a/hubero_interfaces/include/hubero_interfaces/navigation_base.h +++ b/hubero_interfaces/include/hubero_interfaces/navigation_base.h @@ -169,15 +169,29 @@ class NavigationBase { * @brief Transforms local velocity (typically received as velocity command) to a global coordinate system */ static Vector3 convertCommandToGlobalCs(const double& yaw_actor, const Vector3& cmd_vel_local) { - // slide 38 at https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture3-RobotMotion.pdf + // Ref: 19-local-to-global-matrix-form at + // https://automaticaddison.com/how-to-describe-the-rotation-of-a-robot-in-2d/ ignition::math::Matrix3d r( - cos(yaw_actor), 0.0, 0.0, - sin(yaw_actor), 0.0, 0.0, - 0.0, 0.0, 1.0 + +std::cos(yaw_actor), -std::sin(yaw_actor), +0.0, + +std::sin(yaw_actor), +std::cos(yaw_actor), +0.0, + +0.0, +0.0, +1.0 ); return r * cmd_vel_local; } + /** + * @brief Transforms a global velocity to a local/base coordinate system + */ + static Vector3 convertCommandToLocalCs(const double& yaw_actor, const Vector3& vel_global) { + // Ref: last equation in https://automaticaddison.com/how-to-describe-the-rotation-of-a-robot-in-2d/ + ignition::math::Matrix3d r( + +std::cos(yaw_actor), +std::sin(yaw_actor), +0.0, + -std::sin(yaw_actor), +std::cos(yaw_actor), +0.0, + +0.0, +0.0, +1.0 + ); + return r * vel_global; + } + protected: /// @brief Stores initialization indicator flag bool initialized_; diff --git a/hubero_interfaces/package.xml b/hubero_interfaces/package.xml index a96cc365..a693b49c 100644 --- a/hubero_interfaces/package.xml +++ b/hubero_interfaces/package.xml @@ -2,7 +2,7 @@ hubero_interfaces - 0.6.0 + 0.7.0 The hubero_interfaces package Jarosław Karwowski diff --git a/hubero_ros/CMakeLists.txt b/hubero_ros/CMakeLists.txt index ec206c9a..cd6ca40f 100644 --- a/hubero_ros/CMakeLists.txt +++ b/hubero_ros/CMakeLists.txt @@ -30,6 +30,7 @@ include_directories( ) set(HUBERO_NAV_ROS hubero_navigation_ros) +set(HUBERO_TASK_HELPERS hubero_task_helpers) set(HUBERO_TASK_ROS hubero_task_request_ros) set(HUBERO_TASK_ROS_API hubero_task_request_ros_api) set(HUBERO_NODE_ROS hubero_node_ros) @@ -42,6 +43,7 @@ catkin_package( include LIBRARIES ${HUBERO_NAV_ROS} + ${HUBERO_TASK_HELPERS} ${HUBERO_TASK_ROS} ${HUBERO_TASK_ROS_API} ${HUBERO_NODE_ROS} @@ -142,8 +144,17 @@ target_link_libraries(${HUBERO_TASK_ROS_API} ${catkin_LIBRARIES} ) +## Functions helping to develop scenarios requiring time/distance synchronization +add_library(${HUBERO_TASK_HELPERS} + include/${PROJECT_NAME}/task_helpers.h + src/task_helpers.cpp +) +target_link_libraries(${HUBERO_TASK_HELPERS} + ${HUBERO_TASK_ROS_API} +) + ## Install -install(TARGETS ${HUBERO_NAV_ROS} ${HUBERO_TASK_ROS} ${HUBERO_TASK_ROS_API} ${HUBERO_NODE_ROS} ${HUBERO_ROS_STATUS} +install(TARGETS ${HUBERO_NAV_ROS} ${HUBERO_TASK_ROS} ${HUBERO_TASK_ROS_API} ${HUBERO_NODE_ROS} ${HUBERO_ROS_STATUS} ${HUBERO_TASK_HELPERS} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} diff --git a/hubero_ros/config/planning/common/move_base.yaml b/hubero_ros/config/planning/common/move_base.yaml index 1d8fc162..6095690f 100644 --- a/hubero_ros/config/planning/common/move_base.yaml +++ b/hubero_ros/config/planning/common/move_base.yaml @@ -1,8 +1,8 @@ planner_frequency: 1.0 -planner_patience: 5.0 +planner_patience: 0.5 controller_frequency: 2.0 -controller_patience: 10.0 +controller_patience: 3.0 oscillation_timeout: 20.0 oscillation_distance: 0.2 diff --git a/hubero_ros/config/planning/common/recovery_behaviors.yaml b/hubero_ros/config/planning/common/recovery_behaviors.yaml index 76c054a6..6d8cfbc0 100644 --- a/hubero_ros/config/planning/common/recovery_behaviors.yaml +++ b/hubero_ros/config/planning/common/recovery_behaviors.yaml @@ -1,11 +1,9 @@ recovery_behaviors: - - name: 'refresh_vo' - type: 'clear_costmap_recovery/ClearCostmapRecovery' - name: 'costmap_reset_far' type: 'clear_costmap_recovery/ClearCostmapRecovery' - name: 'costmap_reset_close' type: 'clear_costmap_recovery/ClearCostmapRecovery' - - name: 'dummy' + - name: 'costmap_reset' type: 'clear_costmap_recovery/ClearCostmapRecovery' costmap_reset_far: @@ -16,6 +14,6 @@ costmap_reset_close: reset_distance: 1.5 layer_names: "obstacle_laser_layer" #, "obstacle_rgbd_layer"] -dummy: - reset_distance: 5.5 +costmap_reset: + reset_distance: 0.0 layer_names: "obstacle_laser_layer" #, "obstacle_rgbd_layer"] diff --git a/hubero_ros/include/hubero_ros/navigation_ros.h b/hubero_ros/include/hubero_ros/navigation_ros.h index c456767f..67b06ac2 100644 --- a/hubero_ros/include/hubero_ros/navigation_ros.h +++ b/hubero_ros/include/hubero_ros/navigation_ros.h @@ -42,6 +42,17 @@ class NavigationRos: public NavigationBase { */ static const int QUATERNION_RANDOM_RETRY_NUM; + /** + * @detail Defines for how long (since the last action result callback) any feedback of the action won't override + * @ref feedback_ + */ + static const double ACTION_RESULT_FREEZE_TIME_SEC; + + /** + * Maximum duration to keep the same velocity command as a valid one; will publish zero velocity after + */ + static const double CMD_VEL_KEEP_DURATION; + /** * @brief Constructor */ @@ -200,6 +211,8 @@ class NavigationRos: public NavigationBase { ros::Subscriber sub_feedback_; /// @brief Subscriber of the move_base simple action server's result topic ros::Subscriber sub_result_; + /// @brief Stores a timestamp of the last result message + ros::Time cb_result_timestamp_; /// Helper typedefs typedef actionlib::SimpleActionClient MoveBaseActionClient; @@ -253,7 +266,9 @@ class NavigationRos: public NavigationBase { * @{ */ Vector3 cmd_vel_; + ros::Time cmd_vel_timestamp_; std::mutex mutex_callback_; + mutable ros::Time cmd_vel_timeout_log_timestamp_; /// @} /// @brief Initial pose of the actor - used for 'odometry' calculations diff --git a/hubero_ros/include/hubero_ros/task_helpers.h b/hubero_ros/include/hubero_ros/task_helpers.h new file mode 100644 index 00000000..3138b8f8 --- /dev/null +++ b/hubero_ros/include/hubero_ros/task_helpers.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace hubero { + +/** + * @brief Tracks the distance between the actor and another object + */ +class DistanceTracker { +public: + // How often (at most) the log with a distance update will be shown (in seconds) + const double PERIODIC_LOG_TIME = 1.0; + + /** + * @brief Constructor + * + * The user should safely join the thread started in the ctor by calling "getThread().join()" + * + * @param actor a reference HuBeRo actor, a distance between the actor and another object is tracked + * @param object_odom_topic name of the topic with the odometry data of another object + * @param distance_threshold + * @param fun_on_violation function to be executed once the distance threshold is violated + */ + explicit DistanceTracker( + const TaskRequestRosApi& actor, + const std::string& object_odom_topic, + const double& distance_threshold, + std::function& fun_on_violation + ); + + std::thread getThread() { + return std::move(t_); + } + +protected: + std::thread t_; +}; + +} // namespace hubero diff --git a/hubero_ros/include/hubero_ros/task_request_ros_api.h b/hubero_ros/include/hubero_ros/task_request_ros_api.h index ef340ee8..dd379ac1 100644 --- a/hubero_ros/include/hubero_ros/task_request_ros_api.h +++ b/hubero_ros/include/hubero_ros/task_request_ros_api.h @@ -20,7 +20,10 @@ #include #include +#include + #include +#include namespace hubero { @@ -33,12 +36,23 @@ namespace hubero { */ class TaskRequestRosApi { public: + /// Defines the duration of sleeps between subsequent processings of the callback queue + const int CALLBACK_SPINNING_SLEEP_TIME_MS = 10; + + /// Defines the duration of sleeps between subsequent evaluations of task conditions + const int THREADED_EXECUTOR_SLEEP_TIME_MS = 100; + /** * @brief Constructor of @ref TaskRequestRosApi instance * @param actor_name actor identifier from simulation */ TaskRequestRosApi(const std::string& actor_name); + /** + * @brief Destructor that kills all spawned threads + */ + virtual ~TaskRequestRosApi(); + /** * @defgroup tasks Methods that allow to request specific task from controlled actor * @{ @@ -63,8 +77,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getFollowObjectState() const; @@ -108,8 +120,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used - * * @note Currently: preempted -> finished */ TaskFeedbackType getLieDownState() const; @@ -143,8 +153,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getMoveAroundState() const; @@ -186,8 +194,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getMoveToGoalState() const; TaskFeedbackType getMoveToObjectState() const; @@ -198,6 +204,21 @@ class TaskRequestRosApi { std::string getMoveToGoalStateDescription() const; std::string getMoveToObjectStateDescription() const; + /** + * @brief Implements a typical use case where an actor is intended to travel through a given set of waypoints + * + * Execution state can be checked with @ref isThreadExecuting + * This method mustn't be used once another task has been started with the @ref startThreadedExecution + * + * The user must provide that the returned thread object won't go out of scope until the thread is finished. + * Additionally, they should call the std::thread.join() at the end of the scenario executable. + */ + std::thread moveThroughWaypoints( + const std::vector>& poses2d, + const std::string& frame, + const ros::Duration& timeout = ros::Duration(0.0) + ); + /** @} */ // end of movetogoal group /** @@ -220,8 +241,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getRunState() const; @@ -264,8 +283,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getSitDownState() const; TaskFeedbackType getSitDownObjectState() const; @@ -294,8 +311,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getStandState() const; @@ -338,8 +353,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getTalkState() const; TaskFeedbackType getTalkObjectState() const; @@ -372,8 +385,6 @@ class TaskRequestRosApi { /** * @brief Returns most recent state of the task - * - * @details It will return TASK_FEEDBACK_UNDEFINED if @ref ros::spinOnce() is not used */ TaskFeedbackType getTeleopState() const; @@ -392,6 +403,65 @@ class TaskRequestRosApi { return actor_name_; } + /** + * @brief Obtains a pose of the actor in a frame given by @ref frame_reference + */ + Pose3 getPose(const std::string& frame_reference) const; + + /** + * @defgroup execution Methods related to a task execution in a separate thread + */ + /** + * @brief Starts the execution of a single task defined by the arguments in a separate thread + * + * This method does not stop previously started task in any way. + * + * @param state_checker_fun a reference to a function that allows checking the state of task execution, + * e.g., @ref getTeleopState; reference is a must here - inline lambda won't work + * @param logging_task_name task name used in logs + * @param timeout timeout for task execution (by default - no limit) + * @param state_executing a state in which the task is assumed to be executing + */ + void startThreadedExecution( + const std::function& state_checker_fun, + const std::string& logging_task_name, + const ros::Duration& timeout = ros::Duration(0.0), + TaskFeedbackType state_executing = TaskFeedbackType::TASK_FEEDBACK_ACTIVE + ); + + /// Evaluates whether the thread that executes the newest task does still perform computations + inline bool isThreadExecuting() const { + return threaded_task_execution_requested_ && !threaded_task_execution_done_; + } + + /** + * @brief Joins the thread that executes the newest task + * + * Can be also treated as a method that executes a task started with @ref startThreadedExecution in a blocking way + */ + void join(); + + /// @brief Calls @ref std::thread::joinable method on the @ref task_executor_ thread + inline bool joinable() const { + return task_executor_.joinable(); + } + + /** @} */ // end of execution group + + /** + * @defgroup utils Static functions that may be handy when designing a custom scenario + */ + /// Sleeps the current thread for the @ref ms milliseconds + static void wait(const std::chrono::milliseconds& ms = std::chrono::milliseconds(500)); + + /// Sleeps the current thread for the @ref seconds regarding the ROS time + static void waitRosTime(double seconds); + + /// Sleeps the current thread for the @ref duration regarding the ROS time + static void waitRosTime(const ros::Duration& duration); + + /** @} */ // end of utils group + protected: std::shared_ptr node_ptr_; @@ -498,9 +568,53 @@ class TaskRequestRosApi { return action_client_ptr->getFeedbackText(); } + /** + * A method that is executed in a separate thread (@ref callback_spinner_); periodically calls `ros::spinOnce` + * + * This is necessary for ActionServers to receive messages (otherwise is must've been handled by the user in his + * orchestrating node). + */ + virtual void callbackProcessingThread(); + + /** + * Takes care of the execution of a task defined by the arguments. + * + * This should be called immediately after sending a task request. + * + * @param state_checker_fun a function that allows checking the state of task execution, e.g., @ref getTeleopState + * @param logging_task_name task name used in logs + * @param timeout timeout for task execution (by default no limit) + * @param state_executing a state in which the task is assumed to be executing + */ + void threadedExecutor( + const std::function& state_checker_fun, + const std::string& logging_task_name, + const ros::Duration& timeout, + TaskFeedbackType state_executing + ); + private: /// Name of the actor provided in the constructor std::string actor_name_; + /// A separate thread for processing callbacks + std::thread callback_spinner_; + + /// A thread for executing a task + std::thread task_executor_; + /// Atomic flag indicating the request of a threaded execution of a task + std::atomic threaded_task_execution_requested_; + /// Atomic flag indicating the finish of computations for @ref task_executor_ thread + std::atomic threaded_task_execution_done_; + + /// Flag set in the destructor to join threads + std::atomic destructing_; + + /// Buffer for obtaining ROS transforms (i.a. self pose) + tf2_ros::Buffer tf_buffer_; + /// Listener for obtaining ROS transforms (i.a. self pose) + tf2_ros::TransformListener tf_listener_; + /// Name of the TF frame related to the actor + std::string actor_tf_frame_; }; // class TaskRequestRosApi } // namespace hubero diff --git a/hubero_ros/include/hubero_ros/utils/action_client.h b/hubero_ros/include/hubero_ros/utils/action_client.h index a2ad55ed..4a700df1 100644 --- a/hubero_ros/include/hubero_ros/utils/action_client.h +++ b/hubero_ros/include/hubero_ros/utils/action_client.h @@ -9,6 +9,7 @@ #include #include +#include namespace hubero { @@ -36,7 +37,8 @@ class ActionClient: public actionlib::SimpleActionClient { */ ActionClient( std::shared_ptr node_ptr, - const std::string& action_task_ns): + const std::string& action_task_ns + ): actionlib::SimpleActionClient(action_task_ns, true) { // action status subscribers @@ -51,6 +53,7 @@ class ActionClient: public actionlib::SimpleActionClient { &ActionClient::callbackResult, this ); feedback_status_ = TaskFeedbackType::TASK_FEEDBACK_UNDEFINED; + action_task_ns_ = action_task_ns; if (!ros::ok()) { ROS_ERROR("%s: Action server has not been started correctly (ROS is not running)", action_task_ns.c_str()); @@ -77,28 +80,56 @@ class ActionClient: public actionlib::SimpleActionClient { * @{ */ inline TaskFeedbackType getFeedbackStatus() const { + std::lock_guard lock(callback_mutex_); return feedback_status_; } inline std::string getFeedbackText() const { + std::lock_guard lock(callback_mutex_); return feedback_txt_; } /// @} protected: void callbackFeedback(const Tfeedback& msg) { + std::lock_guard lock(callback_mutex_); + + // copy to detect any changes + TaskFeedbackType status_prev = feedback_status_; + std::string txt_prev = feedback_txt_; + feedback_txt_ = std::string(msg->feedback.feedback.text); feedback_status_ = static_cast(msg->feedback.feedback.status); + + if (feedback_txt_ != txt_prev || feedback_status_ != status_prev) { + HUBERO_LOG( + "[ActionClient][%s] Action feedback changed to: {status `%d`, text `%s`}\r\n", + action_task_ns_.c_str(), + feedback_status_, + feedback_txt_.c_str() + ); + } } void callbackResult(const Tresult& msg) { + std::lock_guard lock(callback_mutex_); + feedback_txt_ = std::string(msg->result.result.text); feedback_status_ = static_cast(msg->result.result.status); + + HUBERO_LOG( + "[ActionClient][%s] Received action result: {status `%d`, text `%s`}\r\n", + action_task_ns_.c_str(), + feedback_status_, + feedback_txt_.c_str() + ); } + std::string action_task_ns_; ros::Subscriber sub_feedback_; ros::Subscriber sub_result_; TaskFeedbackType feedback_status_; std::string feedback_txt_; + mutable std::mutex callback_mutex_; }; // class ActionClient } // namespace hubero diff --git a/hubero_ros/package.xml b/hubero_ros/package.xml index 071269c3..97ae49ec 100644 --- a/hubero_ros/package.xml +++ b/hubero_ros/package.xml @@ -1,7 +1,7 @@ hubero_ros - 0.6.0 + 0.7.2 The hubero_ros package Jarosław Karwowski diff --git a/hubero_ros/src/navigation_ros.cpp b/hubero_ros/src/navigation_ros.cpp index 8f57c5a2..7dc24560 100644 --- a/hubero_ros/src/navigation_ros.cpp +++ b/hubero_ros/src/navigation_ros.cpp @@ -15,6 +15,8 @@ namespace hubero { const int NavigationRos::SUBSCRIBER_QUEUE_SIZE = 10; const int NavigationRos::PUBLISHER_QUEUE_SIZE = 15; const int NavigationRos::QUATERNION_RANDOM_RETRY_NUM = 10; +const double NavigationRos::ACTION_RESULT_FREEZE_TIME_SEC = 2.0; +const double NavigationRos::CMD_VEL_KEEP_DURATION = 1.0; NavigationRos::NavigationRos(): NavigationBase::NavigationBase(), @@ -24,7 +26,9 @@ NavigationRos::NavigationRos(): map_x_max_(0.0), map_y_min_(0.0), map_y_max_(0.0), - tf_listener_(tf_buffer_) {} + tf_listener_(tf_buffer_), + cmd_vel_timestamp_(ros::Time(0)), + cmd_vel_timeout_log_timestamp_(ros::Time(0)) {} bool NavigationRos::initialize( std::shared_ptr node_ptr, @@ -222,14 +226,8 @@ void NavigationRos::update(const Pose3& pose, const Vector3& vel_lin, const Vect Pose3 odom_pose = pose - pose_initial_; odometry.pose.pose = ignPoseToMsgPose(odom_pose); setIdealCovariance(odometry.pose.covariance); - // twist - velocities expressed in the base frame - // pseudoinversion of matrix in NavigationBase::convertCommandToGlobalCs - ignition::math::Matrix3d r( - cos(pose.Rot().Yaw()), sin(pose.Rot().Yaw()), 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0 - ); - Vector3 vel_lin_base = r * vel_lin; + // twist - velocities expressed in the base frame -> convert global velocity to the local/base velocity + Vector3 vel_lin_base = NavigationBase::convertCommandToLocalCs(pose.Rot().Yaw(), vel_lin); // assume that Z axis direction matches simulator frame and 'base' frame (typically valid) Vector3 vel_ang_base(0.0, 0.0, vel_ang.Z()); odometry.twist.twist = ignVectorsToMsgTwist(vel_lin_base, vel_ang_base); @@ -506,6 +504,21 @@ Vector3 NavigationRos::getVelocityCmd() const { return Vector3(); } + // evaluate the age of the command + double command_age = (ros::Time::now() - cmd_vel_timestamp_).toSec(); + if (command_age > NavigationRos::CMD_VEL_KEEP_DURATION) { + if ((ros::Time::now() - cmd_vel_timeout_log_timestamp_).toSec() >= 1.0) { + cmd_vel_timeout_log_timestamp_ = ros::Time::now(); + HUBERO_LOG( + "[%s].[NavigationRos] Publishing zero velocity as the latest command is outdated - it's %.1fs old, " + "while the timeout is %.1fs\r\n", + actor_name_.c_str(), + command_age, + NavigationRos::CMD_VEL_KEEP_DURATION + ); + } + return Vector3(); + } return cmd_vel_; } @@ -552,6 +565,7 @@ void NavigationRos::callbackCmdVel(const geometry_msgs::Twist::ConstPtr& msg) { cmd_vel_local.Y(msg->linear.y); cmd_vel_local.Z(msg->angular.z); cmd_vel_ = NavigationBase::convertCommandToGlobalCs(current_pose_.Rot().Yaw(), cmd_vel_local); + cmd_vel_timestamp_ = ros::Time::now(); } void NavigationRos::callbackFeedback(const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg) { @@ -560,6 +574,20 @@ void NavigationRos::callbackFeedback(const move_base_msgs::MoveBaseActionFeedbac if (fb_type == TASK_FEEDBACK_UNDEFINED) { return; } + // The aim is to prevent a late action feedback overriding the result (stored in @ref feedback_) + if ( + (ros::Time::now() - cb_result_timestamp_).toSec() <= NavigationRos::ACTION_RESULT_FREEZE_TIME_SEC + && fb_type == TASK_FEEDBACK_ACTIVE + ) { + HUBERO_LOG( + "[%s].[NavigationRos] Action feedback won't be overwritten because the action result has recently been updated (to %d). " + "Ignored feedback status is %d\r\n", + actor_name_.c_str(), + feedback_, + fb_type + ); + return; + } feedback_ = fb_type; } @@ -570,6 +598,8 @@ void NavigationRos::callbackResult(const move_base_msgs::MoveBaseActionResult::C return; } feedback_ = fb_type; + // save for later use in @ref callbackFeedback + cb_result_timestamp_ = ros::Time::now(); } std::tuple NavigationRos::findTransform(const std::string& frame_source, const std::string& frame_target) const { @@ -710,12 +740,13 @@ nav_msgs::Path NavigationRos::computePlan( } HUBERO_LOG( - "[%s].[NavigationRos] Computed plan with %lu poses (goal: {%2.2f, %2.2f}, " + "[%s].[NavigationRos] Computed plan with %lu poses (goal: {%2.2f, %2.2f, %2.2f}, " "lastElem: {%2.2f, %2.2f}, secToLastElem: {%2.2f, %2.2f}\r\n", actor_name_.c_str(), resp.plan.poses.size(), pose_goal_global_ref_plane.Pos().X(), pose_goal_global_ref_plane.Pos().Y(), + pose_goal_global_ref_plane.Rot().Yaw(), resp.plan.poses.back().pose.position.x, resp.plan.poses.back().pose.position.y, resp.plan.poses.end()[-2].pose.position.x, diff --git a/hubero_ros/src/task_helpers.cpp b/hubero_ros/src/task_helpers.cpp new file mode 100644 index 00000000..f1a3c3ec --- /dev/null +++ b/hubero_ros/src/task_helpers.cpp @@ -0,0 +1,116 @@ +#include + +#include + +namespace hubero { + +DistanceTracker::DistanceTracker( + const TaskRequestRosApi& actor, + const std::string& object_odom_topic, + const double& distance_threshold, + std::function& fun_on_violation +) { + // capture by value only for the DistanceTracker::PERIODIC_LOG_TIME + auto tracker_odom_cb = [=]( + const nav_msgs::Odometry::ConstPtr& odom, + const TaskRequestRosApi& actor, + const double& robot_actor_distance_threshold, + std::atomic& distance_threshold_violated, + const std::function& fun_on_violation, + ros::Time& last_log_time + ) -> void { + if (distance_threshold_violated) { + // nothing more to do + return; + } + + // obtain other's pose + double x_other = odom->pose.pose.position.x; + double y_other = odom->pose.pose.position.y; + // retrieve actor pose in the other frame + auto target_frame = odom->header.frame_id; + // get rid of the "/" at the start of the frame name (forbidden in tf2) + if (!target_frame.empty() && target_frame.at(0) == '/') { + target_frame.erase(target_frame.begin()); + } + double x_actor = actor.getPose(target_frame).Pos().X(); + double y_actor = actor.getPose(target_frame).Pos().Y(); + // compute distance between objects + double dist = std::hypot(x_other - x_actor, y_other - y_actor); + + // logging + auto time_current = ros::Time::now(); + if ((time_current - last_log_time).toSec() >= DistanceTracker::PERIODIC_LOG_TIME) { + HUBERO_LOG( + "[%s].[DistanceTracker] The current distance is %6.3f m (threshold %6.3f m). " + "The position of the actor is {x: %6.3f, y: %6.3f} and the other object's {x: %6.3f, y: %6.3f} " + "(in '%s' frame)\r\n", + actor.getName().c_str(), + dist, + distance_threshold, + x_actor, + y_actor, + x_other, + y_other, + target_frame.c_str() + ); + last_log_time = time_current; + } + + // if distance is small enough, call the function + if (dist < robot_actor_distance_threshold) { + distance_threshold_violated = true; + if (fun_on_violation) { + HUBERO_LOG( + "[%s].[DistanceTracker] The threshold distance (%6.3f m) has been violated (%6.3f m currently). " + "Executing the requested action!\r\n", + actor.getName().c_str(), + distance_threshold, + dist + ); + fun_on_violation(); + } + } + }; + + t_ = std::thread( + [&, tracker_odom_cb, object_odom_topic]() -> void { + // subscribe robot's movement feedback to trigger the start of movement + ros::NodeHandle nh("~"); + // indicates end of the tracking + std::atomic distance_threshold_violated; + distance_threshold_violated = false; + // helper for periodic logging + auto log_time_last = ros::Time::now(); + // in a subscription callback, distance between objects will be updated + ros::Subscriber mb_sub = nh.subscribe( + object_odom_topic, + 1, + std::bind( + std::ref(tracker_odom_cb), + std::placeholders::_1, + std::cref(actor), + std::cref(distance_threshold), + std::ref(distance_threshold_violated), + std::ref(fun_on_violation), + std::ref(log_time_last) + ) + ); + + while (ros::ok() && !distance_threshold_violated) { + ros::spinOnce(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + ); + + HUBERO_LOG( + "[%s].[DistanceTracker] Started measuring distance against the odometry topic '%s' " + "with a distance threshold of %.4f m\r\n", + actor.getName().c_str(), + object_odom_topic.c_str(), + distance_threshold + ); +} + +} // namespace hubero diff --git a/hubero_ros/src/task_request_ros_api.cpp b/hubero_ros/src/task_request_ros_api.cpp index 570fdee7..a350b517 100644 --- a/hubero_ros/src/task_request_ros_api.cpp +++ b/hubero_ros/src/task_request_ros_api.cpp @@ -7,8 +7,13 @@ namespace hubero { TaskRequestRosApi::TaskRequestRosApi(const std::string& actor_name): - node_ptr_(std::make_shared("task_request_ros_api_node") -) { + node_ptr_(std::make_shared("task_request_ros_api_node")), + callback_spinner_(std::thread(&TaskRequestRosApi::callbackProcessingThread, this)), + threaded_task_execution_requested_(false), + threaded_task_execution_done_(false), + destructing_(false), + tf_listener_(tf_buffer_) +{ // assignment - init list does not support string reference copying actor_name_ = actor_name; @@ -110,6 +115,24 @@ TaskRequestRosApi::TaskRequestRosApi(const std::string& actor_name): hubero_ros_msgs::TeleopActionResultConstPtr> >(node_ptr_, actor_task_ns + TaskRequestBase::getTaskName(TASK_TELEOP) ); + + // "path" to obtain the parameter, see the `actor_base_frame` param in the .launch file + std::string param_frame = "/hubero_ros/" + actor_name + "/actor_frames/base"; + node_ptr_->getNodeHandlePtr()->param( + param_frame, + actor_tf_frame_, + std::string(actor_name + "base_footprint") + ); +} + +TaskRequestRosApi::~TaskRequestRosApi() { + destructing_ = true; + if (task_executor_.joinable()) { + task_executor_.join(); + } + if (callback_spinner_.joinable()) { + callback_spinner_.join(); + } } bool TaskRequestRosApi::followObject(const std::string& object_name) { @@ -249,6 +272,58 @@ std::string TaskRequestRosApi::getMoveToObjectStateDescription() const { return getActionStateDescription(ac_move_to_object_ptr_); } +std::thread TaskRequestRosApi::moveThroughWaypoints( + const std::vector>& poses2d, + const std::string& frame, + const ros::Duration& timeout +) { + if (isThreadExecuting()) { + HUBERO_LOG( + "[%s].[TaskRequestRosApi] Could not start 'moveThroughWaypoints' executor as another task is currently " + "executed. Returning a thread that will finish immediately.\r\n", + getName().c_str() + ); + return std::move(std::thread([]() { ; })); + } + + HUBERO_LOG( + "[%s].[TaskRequestRosApi].[moveThroughWaypoints] Requested %lu waypoints\r\n", + getName().c_str(), + poses2d.size() + ); + + std::thread t( + [&, poses2d, frame, timeout]() -> void { + std::function feedback_checker = [&]() -> hubero::TaskFeedbackType { + return getMoveToGoalState(); + }; + + size_t waypoint_num = 1; + for (const auto& waypoint: poses2d) { + auto pos = waypoint.first; + auto yaw = waypoint.second; + HUBERO_LOG( + "[%s].[TaskRequestRosApi].[moveThroughWaypoints] Requesting the %lu/%lu waypoint {x: %5.2f, y: %5.2f, theta: %5.2f}\r\n", + getName().c_str(), + waypoint_num, + poses2d.size(), + pos.X(), + pos.Y(), + yaw + ); + + moveToGoal(pos, yaw, frame); + startThreadedExecution(feedback_checker, "moveToGoal", timeout); + // waiting for the execution to finish + join(); + + waypoint_num++; + } + } + ); + return std::move(t); +} + bool TaskRequestRosApi::run(const Vector3& pos, const double& yaw, const std::string& frame_id) { hubero_ros_msgs::RunGoal action_goal; action_goal.pos = ignVectorToMsgPoint(pos); @@ -405,4 +480,153 @@ std::string TaskRequestRosApi::getTeleopStateDescription() const { return getActionStateDescription(ac_teleop_ptr_); } +Pose3 TaskRequestRosApi::getPose(const std::string& frame_reference) const { + Pose3 transform; + try { + auto tf_msg = tf_buffer_.lookupTransform(frame_reference, actor_tf_frame_, ros::Time(0)); + transform = msgTfToPose(tf_msg.transform); + } catch (tf2::TransformException& ex) { + HUBERO_LOG( + "[%s].[TaskRequestRosApi] Could not transform '%s' to '%s' - exception: '%s'\r\n", + getName().c_str(), + frame_reference.c_str(), + actor_tf_frame_.c_str(), + ex.what() + ); + } + return transform; +} + +void TaskRequestRosApi::startThreadedExecution( + const std::function& state_checker_fun, + const std::string& logging_task_name, + const ros::Duration& timeout, + TaskFeedbackType state_executing +) { + threaded_task_execution_requested_ = true; + threaded_task_execution_done_ = false; + task_executor_ = std::thread( + &TaskRequestRosApi::threadedExecutor, + this, + std::ref(state_checker_fun), + logging_task_name, + timeout, + state_executing + ); +} + +void TaskRequestRosApi::join() { + if (task_executor_.joinable()) { + task_executor_.join(); + } + threaded_task_execution_done_ = true; + threaded_task_execution_requested_ = false; +} + +// static +void TaskRequestRosApi::wait(const std::chrono::milliseconds& ms) { + std::this_thread::sleep_for(ms); +} + +// static +void TaskRequestRosApi::waitRosTime(double seconds) { + waitRosTime(ros::Duration(seconds)); +} + +// static +void TaskRequestRosApi::waitRosTime(const ros::Duration& duration) { + auto time_start_delay = ros::Time::now(); + auto duration_delay = ros::Time::now() - time_start_delay; + while (duration_delay <= duration) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + duration_delay = ros::Time::now() - time_start_delay; + } +} + +void TaskRequestRosApi::callbackProcessingThread() { + while (!destructing_) { + if (!ros::ok()) { + throw std::runtime_error( + "[HuBeRo] [TaskRequestRosApi].[callbackProcessingThread] ROS stopped working, " + "won't process any more callbacks" + ); + } + ros::spinOnce(); + std::this_thread::sleep_for(std::chrono::milliseconds(CALLBACK_SPINNING_SLEEP_TIME_MS)); + } +} + +void TaskRequestRosApi::threadedExecutor( + const std::function& state_checker_fun, + const std::string& logging_task_name, + const ros::Duration& timeout, + TaskFeedbackType state_executing +) { + auto timeout_start = ros::Time::now(); + + // we will be waiting for actor tasks finishes, so we must also know if the task request was processed + while (!destructing_ && state_checker_fun() != state_executing) { + if (!ros::ok()) { + throw std::runtime_error( + "[HuBeRo] [TaskRequestRosApi].[threadedExecutor] ROS stopped working during " + + logging_task_name + + " preparation by the " + + getName() + ); + } + if (!timeout.isZero() && (ros::Time::now() - timeout_start) >= timeout) { + throw std::runtime_error( + "[HuBeRo] [TaskRequestRosApi].[threadedExecutor] Timeout of " + + logging_task_name + + " (preparation) for " + + getName() + + " has elapsed!" + ); + } + HUBERO_LOG( + "[%s].[TaskRequestRosApi] Waiting for the `%s` task to become active. Current state %d...\r\n", + getName().c_str(), + logging_task_name.c_str(), + state_checker_fun() + ); + std::this_thread::sleep_for(std::chrono::milliseconds(THREADED_EXECUTOR_SLEEP_TIME_MS)); + } + + HUBERO_LOG( + "[%s].[TaskRequestRosApi] `%s` task properly activated (state %d), starting the execution...\r\n", + getName().c_str(), + logging_task_name.c_str(), + state_checker_fun() + ); + + while (!destructing_ && state_checker_fun() == state_executing) { + if (!ros::ok()) { + throw std::runtime_error( + "[HuBeRo] [TaskRequestRosApi].[threadedExecutor] ROS stopped working during " + + logging_task_name + + " execution by the " + + getName() + ); + } + if (!timeout.isZero() && (ros::Time::now() - timeout_start) >= timeout) { + throw std::runtime_error( + "[HuBeRo] [TaskRequestRosApi].[threadedExecutor] Timeout of " + + logging_task_name + + " (execution) for " + + getName() + + " has elapsed!" + ); + } + std::this_thread::sleep_for(std::chrono::milliseconds(THREADED_EXECUTOR_SLEEP_TIME_MS)); + } + + HUBERO_LOG( + "[%s].[TaskRequestRosApi] Execution of `%s` task ended with the state %d\r\n", + getName().c_str(), + logging_task_name.c_str(), + state_checker_fun() + ); + threaded_task_execution_done_ = true; +} + } // namespace hubero diff --git a/hubero_ros_scenarios/README.md b/hubero_ros_scenarios/README.md index b995c3e5..6f2f97f3 100644 --- a/hubero_ros_scenarios/README.md +++ b/hubero_ros_scenarios/README.md @@ -24,6 +24,8 @@ In the second terminal, run scenario execution: rosrun hubero_ros_scenarios parking_node 3 ``` +The `parking` scenario is implemented relying on tasks execution in separate threads. + ### `living room` scenario @@ -37,5 +39,4 @@ In the second terminal, run scenario execution: rosrun hubero_ros_scenarios living_room_node 3 ``` -## Notes -If one is creating his custom scenario, just remember to `ros::spinOnce` while waiting for specific task feedback. +The `living room` scenario is implemented relying on manual evaluating of the task states (threaded executor is not used, in contrary to the `parking` scenario). diff --git a/hubero_ros_scenarios/package.xml b/hubero_ros_scenarios/package.xml index bf21c035..9aff15b9 100644 --- a/hubero_ros_scenarios/package.xml +++ b/hubero_ros_scenarios/package.xml @@ -1,7 +1,7 @@ hubero_ros_scenarios - 0.6.0 + 0.7.0 The hubero_ros_scenarios package Jarosław Karwowski diff --git a/hubero_ros_scenarios/src/living_room_node.cpp b/hubero_ros_scenarios/src/living_room_node.cpp index 5ed1ddb7..9dee4163 100644 --- a/hubero_ros_scenarios/src/living_room_node.cpp +++ b/hubero_ros_scenarios/src/living_room_node.cpp @@ -11,12 +11,6 @@ using namespace hubero; const std::string TF_FRAME_REF = "world"; -/// Allows to process any ROS callbacks -void waitRefreshingRos() { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ros::spinOnce(); -} - int main(int argc, char** argv) { // node initialization ros::init(argc, argv, "living_room_scenario_node"); @@ -50,7 +44,7 @@ int main(int argc, char** argv) { return (0); } ROS_INFO("Waiting for Actor2 task to become active. Current state %d...", actor2.getMoveToGoalState()); - waitRefreshingRos(); + hubero::TaskRequestRosApi::wait(); } } @@ -60,7 +54,7 @@ int main(int argc, char** argv) { ROS_INFO("Node stopped!"); return (0); } - waitRefreshingRos(); + hubero::TaskRequestRosApi::wait(); } ROS_INFO("[SCENARIO] 1st stage completed!"); @@ -72,7 +66,6 @@ int main(int argc, char** argv) { actor2.talk(Vector3(+5.5, +1.0, 0.0), IGN_DTOR(75), TF_FRAME_REF); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - ros::spinOnce(); ROS_INFO("Actor2 finishes talking to the person nearby!"); actor2.stopTalking(); @@ -98,7 +91,7 @@ int main(int argc, char** argv) { actor1.getMoveToGoalState(), actor2.getMoveToGoalState() ); - waitRefreshingRos(); + hubero::TaskRequestRosApi::wait(); } } @@ -114,7 +107,7 @@ int main(int argc, char** argv) { ROS_INFO("Node stopped!"); return (0); } - waitRefreshingRos(); + hubero::TaskRequestRosApi::wait(); } ROS_INFO("[SCENARIO] 3rd stage completed!"); diff --git a/hubero_ros_scenarios/src/parking_node.cpp b/hubero_ros_scenarios/src/parking_node.cpp index ad207116..075123d1 100644 --- a/hubero_ros_scenarios/src/parking_node.cpp +++ b/hubero_ros_scenarios/src/parking_node.cpp @@ -11,12 +11,6 @@ using namespace hubero; const std::string TF_FRAME_REF = "world"; -/// Allows to process any ROS callbacks -void waitRefreshingRos() { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ros::spinOnce(); -} - int main(int argc, char** argv) { // node initialization ros::init(argc, argv, "parking_scenario_node"); @@ -36,7 +30,21 @@ int main(int argc, char** argv) { // wait ROS_INFO("TaskRequestRos APIs fired up, scenario execution will start in %lu seconds", launch_delay / 1000); - std::this_thread::sleep_for(std::chrono::milliseconds(launch_delay)); + hubero::TaskRequestRosApi::waitRosTime(launch_delay / 1000); + + // preparation + std::function actor4_move_feedback_checker = [&actor4]() -> hubero::TaskFeedbackType { + return actor4.getMoveToGoalState(); + }; + std::function actor3_move_feedback_checker = [&actor3]() -> hubero::TaskFeedbackType { + return actor3.getMoveToGoalState(); + }; + std::function actor2_move_feedback_checker = [&actor2]() -> hubero::TaskFeedbackType { + return actor2.getMoveToGoalState(); + }; + std::function actor1_move_feedback_checker = [&actor1]() -> hubero::TaskFeedbackType { + return actor1.getMoveToGoalState(); + }; // =================== 1st stage ======================================== /* Actor4 and Actor3 go straight to their cars; @@ -48,38 +56,24 @@ int main(int argc, char** argv) { actor2.moveToGoal(Vector3(+7.0, +2.9, 0.0), 0.0, TF_FRAME_REF); actor1.moveToGoal(Vector3(+1.2, -5.7, 0.0), IGN_PI, TF_FRAME_REF); + actor4.startThreadedExecution(std::cref(actor4_move_feedback_checker), "moveToGoal"); + actor3.startThreadedExecution(std::cref(actor3_move_feedback_checker), "moveToGoal"); + actor2.startThreadedExecution(std::cref(actor2_move_feedback_checker), "moveToGoal"); + actor1.startThreadedExecution(std::cref(actor1_move_feedback_checker), "moveToGoal"); + ROS_INFO("[SCENARIO] 1st stage completed!"); // =================== 2nd stage ======================================== - // we will be waiting for actor2's task finish, so we must also know if the task request was processed - if (actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - while (actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - ROS_INFO("Waiting for Actor2 task to become active. Current state %d...", actor2.getMoveToGoalState()); - waitRefreshingRos(); - } - } - ROS_INFO("Task of Actor2 is active! Waiting until he finishes 'moveToGoal'"); - while (actor2.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - waitRefreshingRos(); - } + ROS_INFO("Waiting until Actor2 finishes 'moveToGoal'"); + actor2.join(); ROS_INFO("[SCENARIO] Firing up the 2nd stage!"); /* Actor2 talks for few seconds */ ROS_INFO("Actor2 got to the place! He will start talking to the person nearby!"); actor2.talk(Vector3(+7.0, +2.9, 0.0), IGN_DTOR(90), TF_FRAME_REF); - - std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - ros::spinOnce(); + hubero::TaskRequestRosApi::wait(std::chrono::milliseconds(5000)); ROS_INFO("Actor2 finishes talking to the person nearby!"); actor2.stopTalking(); @@ -88,26 +82,9 @@ int main(int argc, char** argv) { // =================== 3rd stage ======================================== - // actor1 will surely go for a longer time than actor2 - if (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - while (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - ROS_INFO("Waiting for Actor1 task to become active. Current state %d...", actor2.getMoveToGoalState()); - waitRefreshingRos(); - } - } - - ROS_INFO("Task of Actor1 is active! Waiting until he finishes 'moveToGoal'"); - while (actor1.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE) { - if ( !ros::ok() ) { - ROS_INFO("Node stopped!"); - return (0); - } - waitRefreshingRos(); - } + // actor1 will most likely go for a longer time than actor2 + ROS_INFO("Waiting until Actor1 finishes 'moveToGoal'"); + actor1.join(); ROS_INFO("Actor1 threw the rubbish away to the dumpster!"); ROS_INFO("[SCENARIO] Firing up the 3rd stage!"); @@ -116,34 +93,16 @@ int main(int argc, char** argv) { ROS_INFO("Actor1 and Actor2 start moving towards their cars!"); actor2.moveToGoal(Vector3(-3.5, +10.5, 0.0), IGN_PI, TF_FRAME_REF); actor1.moveToGoal(Vector3(-0.5, +14.8, 0.0), IGN_PI, TF_FRAME_REF); + actor2.startThreadedExecution(actor2_move_feedback_checker, "moveToGoal"); + actor1.startThreadedExecution(actor1_move_feedback_checker, "moveToGoal"); - // wait until request processed - if (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE || actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - while (actor1.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE || actor2.getMoveToGoalState() != TASK_FEEDBACK_ACTIVE) { - if (!ros::ok()) { - ROS_INFO("Node stopped!"); - return (0); - } - ROS_INFO( - "Waiting for Actor1 or Actor2 task to become active. Current state: A1 %d, A2 %d...", - actor1.getMoveToGoalState(), - actor2.getMoveToGoalState() - ); - waitRefreshingRos(); - } - } - + // the requests are processed is separate threads ROS_INFO("Actor1 and Actor2 are moving towards cars! Waiting until they reach their goal"); - while ( - actor1.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE - || actor2.getMoveToGoalState() == TASK_FEEDBACK_ACTIVE - ) { - if ( !ros::ok() ) { - ROS_INFO("Node stopped!"); - return (0); - } - waitRefreshingRos(); - } + actor1.join(); + actor2.join(); + + actor3.join(); + actor4.join(); ROS_INFO("[SCENARIO] 3rd stage completed!");