From 7e5b0f6f930f8c172635d24ba2fc656722a22d1e Mon Sep 17 00:00:00 2001 From: philippewarren Date: Thu, 20 Jan 2022 09:58:35 -0500 Subject: [PATCH 1/8] Added skeleton for label image drawer --- map_image_generator/CMakeLists.txt | 2 + .../include/map_image_generator/Parameters.h | 12 ++++++ .../drawers/LabelImageDrawer.h | 22 ++++++++++ map_image_generator/src/MapImageGenerator.cpp | 7 ++++ map_image_generator/src/Parameters.cpp | 3 ++ .../src/drawers/LabelImageDrawer.cpp | 41 +++++++++++++++++++ 6 files changed, 87 insertions(+) create mode 100644 map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h create mode 100644 map_image_generator/src/drawers/LabelImageDrawer.cpp diff --git a/map_image_generator/CMakeLists.txt b/map_image_generator/CMakeLists.txt index b43bf7f..933126a 100644 --- a/map_image_generator/CMakeLists.txt +++ b/map_image_generator/CMakeLists.txt @@ -134,6 +134,7 @@ add_executable(map_image_generator include/map_image_generator/drawers/GlobalPathImageDrawer.h include/map_image_generator/drawers/GoalImageDrawer.h include/map_image_generator/drawers/ImageDrawer.h + include/map_image_generator/drawers/LabelImageDrawer.h include/map_image_generator/drawers/LaserScanImageDrawer.h include/map_image_generator/drawers/OccupancyGridImageDrawer.h include/map_image_generator/drawers/RobotImageDrawer.h @@ -145,6 +146,7 @@ add_executable(map_image_generator src/drawers/GlobalPathImageDrawer.cpp src/drawers/GoalImageDrawer.cpp src/drawers/ImageDrawer.cpp + src/drawers/LabelImageDrawer.cpp src/drawers/LaserScanImageDrawer.cpp src/drawers/OccupancyGridImageDrawer.cpp src/drawers/RobotImageDrawer.cpp diff --git a/map_image_generator/include/map_image_generator/Parameters.h b/map_image_generator/include/map_image_generator/Parameters.h index 342dc58..50f36c0 100644 --- a/map_image_generator/include/map_image_generator/Parameters.h +++ b/map_image_generator/include/map_image_generator/Parameters.h @@ -28,6 +28,7 @@ namespace map_image_generator bool m_drawRobot; bool m_drawGoals; bool m_drawLaserScan; + bool m_drawLabels; cv::Vec3b m_wallColor; cv::Vec3b m_freeSpaceColor; @@ -37,11 +38,13 @@ namespace map_image_generator cv::Scalar m_goalColor; cv::Scalar m_laserScanColor; cv::Scalar m_textColor; + cv::Scalar m_labelColor; int m_globalPathThickness; // pixel int m_robotSize; // pixel int m_goalSize; // pixel int m_laserScanSize; // pixel + int m_labelSize; // pixel public: explicit Parameters(ros::NodeHandle& nodeHandle); @@ -69,6 +72,7 @@ namespace map_image_generator bool drawRobot() const; bool drawGoals() const; bool drawLaserScan() const; + bool drawLabels() const; const cv::Vec3b& wallColor() const; const cv::Vec3b& freeSpaceColor() const; @@ -78,11 +82,13 @@ namespace map_image_generator const cv::Scalar& goalColor() const; const cv::Scalar& laserScanColor() const; const cv::Scalar& textColor() const; + const cv::Scalar& labelColor() const; int globalPathThickness() const; // pixel int robotSize() const; // pixel int goalSize() const; // pixel int laserScanSize() const; // pixel + int labelSize() const; // pixel private: void validateParameters(); @@ -135,6 +141,8 @@ namespace map_image_generator inline bool Parameters::drawLaserScan() const { return m_drawLaserScan; } + inline bool Parameters::drawLabels() const { return m_drawLabels; } + inline const cv::Vec3b& Parameters::wallColor() const { return m_wallColor; } inline const cv::Vec3b& Parameters::freeSpaceColor() const @@ -163,6 +171,8 @@ namespace map_image_generator inline const cv::Scalar& Parameters::textColor() const { return m_textColor; } + inline const cv::Scalar& Parameters::labelColor() const { return m_labelColor; } + inline int Parameters::globalPathThickness() const { return m_globalPathThickness; } inline int Parameters::robotSize() const { return m_robotSize; } @@ -170,5 +180,7 @@ namespace map_image_generator inline int Parameters::goalSize() const { return m_goalSize; } inline int Parameters::laserScanSize() const { return m_laserScanSize; } + + inline int Parameters::labelSize() const { return m_labelSize; } } #endif diff --git a/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h new file mode 100644 index 0000000..4ddf362 --- /dev/null +++ b/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h @@ -0,0 +1,22 @@ +#ifndef LABEL_IMAGE_DRAWER_H +#define LABEL_IMAGE_DRAWER_H + +#include "map_image_generator/drawers/ImageDrawer.h" + +namespace map_image_generator +{ + class LabelImageDrawer : public ImageDrawer + { + public: + LabelImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener); + ~LabelImageDrawer() override; + + void draw(cv::Mat& image) override; + + private: + void drawLabel(const geometry_msgs::PoseStamped& label, const std::string& text, + cv::Mat& image, tf::Transform& transform); + }; +} +#endif diff --git a/map_image_generator/src/MapImageGenerator.cpp b/map_image_generator/src/MapImageGenerator.cpp index 034a94d..839bc22 100644 --- a/map_image_generator/src/MapImageGenerator.cpp +++ b/map_image_generator/src/MapImageGenerator.cpp @@ -2,6 +2,7 @@ #include "map_image_generator/drawers/GlobalPathImageDrawer.h" #include "map_image_generator/drawers/GoalImageDrawer.h" +#include "map_image_generator/drawers/LabelImageDrawer.h" #include "map_image_generator/drawers/LaserScanImageDrawer.h" #include "map_image_generator/drawers/OccupancyGridImageDrawer.h" #include "map_image_generator/drawers/RobotImageDrawer.h" @@ -33,6 +34,12 @@ MapImageGenerator::MapImageGenerator(Parameters& parameters, ros::NodeHandle& no std::make_unique(m_parameters, nodeHandle, m_tfListener)); } + if (m_parameters.drawLabels()) + { + m_drawers.push_back( + std::make_unique(m_parameters, nodeHandle, m_tfListener)); + } + if (m_parameters.drawGoals()) { m_drawers.push_back( diff --git a/map_image_generator/src/Parameters.cpp b/map_image_generator/src/Parameters.cpp index 8b348c9..d56ec4a 100644 --- a/map_image_generator/src/Parameters.cpp +++ b/map_image_generator/src/Parameters.cpp @@ -87,6 +87,7 @@ Parameters::Parameters(ros::NodeHandle& nodeHandle) : m_scaleFactor{1.0} m_drawRobot = getParam("draw_robot", true); m_drawGoals = getParam("draw_goals", true); m_drawLaserScan = getParam("draw_laser_scan", true); + m_drawLabels = getParam("draw_labels", true); m_wallColor = getParam("wall_color", "0 0 0"); m_freeSpaceColor = getParam("free_space_color", "255 255 255"); @@ -96,11 +97,13 @@ Parameters::Parameters(ros::NodeHandle& nodeHandle) : m_scaleFactor{1.0} m_goalColor = getParam("goal_color", "0 175 0 255"); m_laserScanColor = getParam("laser_scan_color", "255 0 0 255"); m_textColor = getParam("text_color", "255 255 255 255"); + m_labelColor = getParam("label_color", "255 0 255 255"); m_globalPathThickness = getParam("global_path_thickness", 3); m_robotSize = getParam("robot_size", 30); m_goalSize = getParam("goal_size", 20); m_laserScanSize = getParam("laser_scan_size", 6); + m_labelSize = getParam("label_size", 35); m_centeredRobot = getParam("centered_robot", true); diff --git a/map_image_generator/src/drawers/LabelImageDrawer.cpp b/map_image_generator/src/drawers/LabelImageDrawer.cpp new file mode 100644 index 0000000..9354c02 --- /dev/null +++ b/map_image_generator/src/drawers/LabelImageDrawer.cpp @@ -0,0 +1,41 @@ +#include "map_image_generator/drawers/LabelImageDrawer.h" + +#include + +using namespace map_image_generator; + +LabelImageDrawer::LabelImageDrawer(const Parameters& parameters, + ros::NodeHandle& nodeHandle, + tf::TransformListener& tfListener) + : ImageDrawer(parameters, nodeHandle, tfListener) +{ +} + +LabelImageDrawer::~LabelImageDrawer() = default; + +void LabelImageDrawer::draw(cv::Mat& image) {} + +void LabelImageDrawer::drawLabel(const geometry_msgs::PoseStamped& label, + const std::string& text, cv::Mat& image, + tf::Transform& transform) +{ + const cv::Scalar& color = m_parameters.labelColor(); + int size = m_parameters.labelSize(); + + tf::Pose labelPose; + tf::poseMsgToTF(label.pose, labelPose); + labelPose = transform * labelPose; + adjustTransformForRobotRef(labelPose); + double yaw = tf::getYaw(labelPose.getRotation()); + + int startX, startY; + convertTransformToMapCoordinates(labelPose, startX, startY); + + int endX = static_cast(startX + size * cos(yaw)); + int endY = static_cast(startY + size * sin(yaw)); + + cv::circle(image, cv::Point(startX, startY), static_cast(ceil(size / 5.0)), + color, cv::FILLED); + cv::putText(image, text, cv::Point(startX, startY), cv::FONT_HERSHEY_DUPLEX, 0.5, + m_parameters.textColor(), 1); +} From 4e6c369dfbdf76dd0ba97492951d4c2a1697800a Mon Sep 17 00:00:00 2001 From: philippewarren Date: Thu, 20 Jan 2022 16:44:21 -0500 Subject: [PATCH 2/8] WIP Added a labels_manager --- .github/workflows/pull-request-audit.yml | 117 ++++++++-------- .gitignore | 4 + .../launch/opentera_turtlebot_sim.launch | 2 + opentera_webrtc_ros/README.md | 60 +++++--- opentera_webrtc_ros/requirements.txt | 1 + opentera_webrtc_ros/scripts/goal_manager.py | 35 +---- opentera_webrtc_ros/scripts/labels_manager.py | 131 ++++++++++++++++++ .../scripts/libmapimageconverter.py | 61 ++++++++ .../scripts/libyamldatabase.py | 87 ++++++++++++ opentera_webrtc_ros/setup.py | 9 +- opentera_webrtc_ros_msgs/CMakeLists.txt | 10 +- opentera_webrtc_ros_msgs/msg/Label.msg | 3 + opentera_webrtc_ros_msgs/msg/LabelArray.msg | 1 + opentera_webrtc_ros_msgs/msg/LabelEdit.msg | 2 + opentera_webrtc_ros_msgs/msg/LabelSimple.msg | 3 + .../msg/LabelSimpleArray.msg | 1 + .../msg/LabelSimpleEdit.msg | 2 + opentera_webrtc_ros_msgs/package.xml | 3 + 18 files changed, 422 insertions(+), 110 deletions(-) create mode 100755 opentera_webrtc_ros/scripts/labels_manager.py create mode 100755 opentera_webrtc_ros/scripts/libmapimageconverter.py create mode 100644 opentera_webrtc_ros/scripts/libyamldatabase.py create mode 100644 opentera_webrtc_ros_msgs/msg/Label.msg create mode 100644 opentera_webrtc_ros_msgs/msg/LabelArray.msg create mode 100644 opentera_webrtc_ros_msgs/msg/LabelEdit.msg create mode 100644 opentera_webrtc_ros_msgs/msg/LabelSimple.msg create mode 100644 opentera_webrtc_ros_msgs/msg/LabelSimpleArray.msg create mode 100644 opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg diff --git a/.github/workflows/pull-request-audit.yml b/.github/workflows/pull-request-audit.yml index 0ed7034..fc0fa21 100644 --- a/.github/workflows/pull-request-audit.yml +++ b/.github/workflows/pull-request-audit.yml @@ -2,73 +2,70 @@ name: pull-request-audit on: push: - branches: [ main ] - + branches: [main] + pull_request: - branches: [ main ] - + branches: [main] + workflow_dispatch: - branches: [ main ] + branches: [main] jobs: build: - runs-on: ubuntu-20.04 - + steps: - - uses: actions/checkout@v2 - with: - submodules: recursive - path: catkin_ws/src/opentera-webrtc-ros - - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: noetic - - - uses: actions/setup-node@v2 - with: - node-version: '14' - - - name: Install system dependencies - run: | - sudo apt-get update - sudo apt-get install nodejs ros-noetic-turtlebot3 ros-noetic-turtlebot3-gazebo ros-noetic-dwa-local-planner ros-noetic-rtabmap-ros - sudo apt-get install libglib2.0-dev libgtk-3-dev libpulse-dev libasound2-dev - sudo apt-get install python3-pip portaudio19-dev - sudo apt-get install nodejs npm - sudo apt-get install build-essential gfortran texinfo libasound2-dev - - - name: Clone audio_utils in ROS workspace - working-directory: catkin_ws/src - run: | - source /opt/ros/noetic/setup.bash - git clone https://github.com/introlab/audio_utils.git --recurse-submodules - ls -l - - - name: Install Python requirements for OpenTera client - working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_client_ros - run: | - ls -l - python3 -m pip install -r requirements.txt - - - name: Install Python requirements for Signaling server - working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_ros/opentera-webrtc/signaling-server - run: | - ls -l - python3 -m pip install -r requirements.txt + - uses: actions/checkout@v2 + with: + submodules: recursive + path: catkin_ws/src/opentera-webrtc-ros + - uses: ros-tooling/setup-ros@v0.2 + with: + required-ros-distributions: noetic + + - uses: actions/setup-node@v2 + with: + node-version: "14" + + - name: Install system dependencies + run: | + sudo apt-get update + sudo apt-get install nodejs ros-noetic-turtlebot3 ros-noetic-turtlebot3-gazebo ros-noetic-dwa-local-planner ros-noetic-rtabmap-ros + sudo apt-get install libglib2.0-dev libgtk-3-dev libpulse-dev libasound2-dev + sudo apt-get install python3-pip portaudio19-dev + sudo apt-get install nodejs npm + sudo apt-get install build-essential gfortran texinfo libasound2-dev + + - name: Clone audio_utils in ROS workspace + working-directory: catkin_ws/src + run: | + source /opt/ros/noetic/setup.bash + git clone https://github.com/introlab/audio_utils.git --recurse-submodules + ls -l + + - name: Install Python requirements for OpenTera client + working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_client_ros + run: | + ls -l + python3 -m pip install -r requirements.txt - - name: Install the VUE.js frontend - working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_demos/opentera-webrtc-teleop-frontend/teleop-vue - run: | - ls -l - npm --version - node --version - npm install - npm run build - - - name: Compile packages in ROS workspace - working-directory: catkin_ws - run: | - source /opt/ros/noetic/setup.bash - catkin_make -j1 + - name: Install Python requirements for Signaling server + working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_ros/opentera-webrtc/signaling-server + run: | + ls -l + python3 -m pip install -r requirements.txt + - name: Install the VUE.js frontend + working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_demos/opentera-webrtc-teleop-frontend/teleop-vue + run: | + ls -l + npm --version + node --version + npm install + npm run build + - name: Compile packages in ROS workspace + working-directory: catkin_ws + run: | + source /opt/ros/noetic/setup.bash + catkin_make -j1 diff --git a/.gitignore b/.gitignore index 259148f..21640c0 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,7 @@ *.exe *.out *.app + +# Compiled Python +__pycache__ +*.pyc diff --git a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch index 5c8e9bc..fb4ab84 100644 --- a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch +++ b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch @@ -107,4 +107,6 @@ + + diff --git a/opentera_webrtc_ros/README.md b/opentera_webrtc_ros/README.md index 299816c..d4c6677 100644 --- a/opentera_webrtc_ros/README.md +++ b/opentera_webrtc_ros/README.md @@ -1,18 +1,22 @@ # Installation The following ROS packages are required: -* roscpp -* cv_bridge -* std_msgs -* sensor_msgs + +- roscpp +- cv_bridge +- std_msgs +- sensor_msgs Also add the following [repository](https://github.com/introlab/audio_utils) in the catkin workspace src directory: + ```bash git clone https://github.com/introlab/audio_utils.git --recurse-submodules ``` + See https://github.com/introlab/audio_utils for more informations about the dependency and his usage; # Clone the repository with the submodules in the catkin workspace src directory + ```bash git clone https://github.com/introlab/opentera-webrtc-ros.git --recurse-submodules ``` @@ -21,7 +25,6 @@ git clone https://github.com/introlab/opentera-webrtc-ros.git --recurse-submodul Complete the following documentation. - Finally the OpenTera WebRTC native client and its dependencies must have been built with same build type, Debug or Release as the desired build output. @@ -34,13 +37,13 @@ It also forwards images and audio received on the WebRTC stream to ROS. ### Subscribes -* ros_image : `sensor_msgs::Image` +- ros_image : `sensor_msgs::Image` ### Advertises -* webrtc_image : `opentera_webrtc_ros::PeerImage` -* webrtc_audio : `opentera_webrtc_ros::PeerAudio` -* audio_mixed : `audio_utils::AudioFrame` +- webrtc_image : `opentera_webrtc_ros::PeerImage` +- webrtc_audio : `opentera_webrtc_ros::PeerAudio` +- audio_mixed : `audio_utils::AudioFrame` ## Default Parameters @@ -65,6 +68,7 @@ It also forwards images and audio received on the WebRTC stream to ROS. } ``` + For usage exemple look at [ros_stream_client.launch](launch/ros_stream_client.launch). # RosDataChannelBridge @@ -76,11 +80,11 @@ data channel. It also forwards messages received on the WebRTC data channel to R ### Subscribes -* ros_data : `std_msgs::String` +- ros_data : `std_msgs::String` ### Advertises -* webrtc_data : `opentera_webrtc_ros_msgs::PeerData` +- webrtc_data : `opentera_webrtc_ros_msgs::PeerData` ## Default Parameters @@ -95,6 +99,7 @@ data channel. It also forwards messages received on the WebRTC data channel to R } ``` + For usage exemple look at [ros_data_channel_client.launch](launch/ros_data_channel_client.launch). # RosJsonDataHandler @@ -105,26 +110,49 @@ Implement a ROS node that dispatch received JSON messages and forward them on th ### Subscribes -* webrtc_data : `opentera_webrtc_ros_msgs::PeerData` +- webrtc_data : `opentera_webrtc_ros_msgs::PeerData` ### Advertises -* cmd_vel : `geometry_msgs::Twist` +- cmd_vel : `geometry_msgs::Twist` ## JSON Format (TODO) For usage exemple look at [ros_json_data_handler.launch](launch/ros_json_data_handler.launch). # goal_manager + ## Description + Manages multiple waypoints received by the frontend and sends them to move_base one at a time. This node relies a service provided by the `map_image_generator` package to convert the waypoints from image coordinates to map coordinates. ## Subscribed topics -* waypoints (opentera_webrtc_ros_msgs/Waypoints): Array of image coordinate waypoints received from the frontend. -* stop (std_msgs/Bool): Signal to cancel all move_base goals. + +- waypoints (`opentera_webrtc_ros_msgs/Waypoints`): Array of image coordinate waypoints received from the frontend. +- stop (`std_msgs/Bool`): Signal to cancel all move_base goals. ## Published topics -* waypoint_reached (std_msgs/String): String of a JSON message containing the ID of the waypoint that has been reached. Used by the frontend to determine when the trajectory has been completed. +- waypoint_reached (`std_msgs/String`): String of a JSON message containing the ID of the waypoint that has been reached. Used by the frontend to determine when the trajectory has been completed. +# labels_manager + +## Description + +Manages labels. +The stored labels are dependent on the `map` transform and the database needs to be cleaned if the map changes. +A label represents a name grouped with an associated pose and a description. +This node relies on a service provided by the `map_image_generator` package to convert the labels from image coordinates to map coordinates. + + + +## Subscribed topics + +- add_label_simple (`opentera_webrtc_ros_msgs/LabelSimple`): Label received from the frontend. +- remove_label_by_name (`std_msgs/String`): Remove a label by its name +- edit_label_simple (`opentera_webrtc_ros_msgs/LabelSimpleEdit`): Rename or move a label using frontend map coordinates + +## Published topics +- stored_labels (`opentera_webrtc_ros_msgs/LabelArray`): Array of labels currently stored + diff --git a/opentera_webrtc_ros/requirements.txt b/opentera_webrtc_ros/requirements.txt index 83a4f48..b55aa05 100644 --- a/opentera_webrtc_ros/requirements.txt +++ b/opentera_webrtc_ros/requirements.txt @@ -1,2 +1,3 @@ pyaudio numpy +ruamel.yaml diff --git a/opentera_webrtc_ros/scripts/goal_manager.py b/opentera_webrtc_ros/scripts/goal_manager.py index 7bd6c7c..2f69242 100755 --- a/opentera_webrtc_ros/scripts/goal_manager.py +++ b/opentera_webrtc_ros/scripts/goal_manager.py @@ -7,9 +7,9 @@ from geometry_msgs.msg import PoseStamped from opentera_webrtc_ros_msgs.msg import WaypointArray from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from map_image_generator.srv import ImageGoalToMapGoal from std_msgs.msg import Bool, String from std_srvs.srv import SetBool +from libmapimageconverter import convert_waypoint_to_pose class GoalManager(): @@ -41,34 +41,11 @@ def __init__(self): def waypoints_cb(self, msg): for waypoint in msg.waypoints: - pose_goal = self.transform_waypoint_to_pose(waypoint) - pose_goal.pose.position.z = 1 + len(self.pose_goals) - self.add_waypoint_to_image_pub.publish(pose_goal) - self.pose_goals.append(pose_goal) - - def transform_waypoint_to_pose(self, waypoint): - pose = PoseStamped() - pose.header.frame_id = "map" - pose.pose.position.x = waypoint.x - pose.pose.position.y = waypoint.y - yaw = -waypoint.yaw - quaternion = quaternion_from_euler(0, 0, yaw) - pose.pose.orientation.x = quaternion[0] - pose.pose.orientation.y = quaternion[1] - pose.pose.orientation.z = quaternion[2] - pose.pose.orientation.w = quaternion[3] - - return self.image_goal_to_map_goal_client(pose) - - def image_goal_to_map_goal_client(self, image_goal): - rospy.wait_for_service('image_goal_to_map_goal') - try: - image_goal_to_map_goal = rospy.ServiceProxy( - 'image_goal_to_map_goal', ImageGoalToMapGoal) - res = image_goal_to_map_goal(image_goal) - return res.map_goal - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) + pose_goal = convert_waypoint_to_pose(waypoint) + if pose_goal is not None: + pose_goal.pose.position.z = 1 + len(self.pose_goals) + self.add_waypoint_to_image_pub.publish(pose_goal) + self.pose_goals.append(pose_goal) def send_goal(self, pose_goal): goal = MoveBaseGoal() diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py new file mode 100755 index 0000000..4b797ac --- /dev/null +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +from __future__ import annotations + +import rospy +from rospy_message_converter.message_converter import convert_ros_message_to_dictionary as ros2dict +from rospy_message_converter.message_converter import convert_dictionary_to_ros_message as dict2ros +from opentera_webrtc_ros_msgs.msg import LabelSimple, LabelSimpleArray, LabelSimpleEdit +from opentera_webrtc_ros_msgs.msg import Label, LabelArray, LabelEdit +from opentera_webrtc_ros_msgs.msg import Waypoint +from std_msgs.msg import String +from geometry_msgs.msg import PoseStamped +from libmapimageconverter import convert_waypoint_to_pose as wp2pose +from libmapimageconverter import convert_pose_to_waypoint as pose2wp + +from libyamldatabase import YamlDatabase +from pathlib import Path +from typing import cast + + +class ConversionError(Exception): + pass + + +class LabelData: + yaml_tag = "!label" + + def __init__(self, label: Label) -> None: + self.data = ros2dict(label) + + @property + def label(self) -> Label: + pose = PoseStamped() + pose.header.frame_id = str(self.data["pose"]["header"]["frame_id"]) + pose.pose.position.x = float( + self.data["pose"]["pose"]["position"]["x"]) + pose.pose.position.y = float( + self.data["pose"]["pose"]["position"]["y"]) + pose.pose.position.z = float( + self.data["pose"]["pose"]["position"]["z"]) + pose.pose.orientation.x = float( + self.data["pose"]["pose"]["orientation"]["x"]) + pose.pose.orientation.y = float( + self.data["pose"]["pose"]["orientation"]["y"]) + pose.pose.orientation.z = float( + self.data["pose"]["pose"]["orientation"]["z"]) + pose.pose.orientation.w = float( + self.data["pose"]["pose"]["orientation"]["w"]) + + return Label(name=self.data["name"], description=self.data["description"], pose=pose) + + +class LabelsManager: + def __init__(self) -> None: + + self.add_label_simple_sub = rospy.Subscriber( + "add_label_simple", LabelSimple, self.add_label_simple_callback, queue_size=1) + self.remove_label_by_name_sub = rospy.Subscriber( + "remove_label_by_name", String, self.remove_label_by_name_callback, queue_size=1) + self.edit_label_simple_sub = rospy.Subscriber( + "edit_label_simple", LabelSimpleEdit, self.edit_label_simple_callback, queue_size=1) + + self.stored_labels_pub = rospy.Publisher( + "stored_labels", LabelArray, queue_size=1) + + self.database_path: str = rospy.get_param( + "~database_path", "~/.ros/labels.yaml") + self.db: YamlDatabase[LabelData] = YamlDatabase( + Path(self.database_path), LabelData) + + self.pub_timer = rospy.Timer(rospy.Duration( + 1), self.publish_stored_labels_simple) + + rospy.loginfo("Labels manager initialized") + + def publish_stored_labels_simple(self, _: rospy.timer.TimerEvent) -> None: + labels = tuple(e.label for e in self.db.values()) + self.stored_labels_pub.publish(labels) + + def add_label_simple_callback(self, msg: LabelSimple) -> None: + try: + label = LabelData(self.simple2label(msg)) + self.db.add(msg.name, label) + self.db.commit() + except (IndexError, ConversionError) as e: + rospy.logerr(f"Adding label to database failed: {e}") + + def remove_label_by_name_callback(self, msg: String) -> None: + try: + self.db.remove(msg.data) + self.db.commit() + except IndexError as e: + rospy.logerr(f"Removing label from database failed: {e}") + + def edit_label_simple_callback(self, msg: LabelSimpleEdit) -> None: + try: + self.db.replace(msg.current_name, LabelData( + self.simple2label(msg.updated))) + if msg.current_name != msg.updated.name: + self.db.rename(msg.current_name, msg.updated.name) + self.db.commit() + except (IndexError, ConversionError) as e: + rospy.logerr(f"Editing label in database failed: {e}") + + @staticmethod + def label2simple(label: Label) -> LabelSimple: + waypoint = pose2wp(label.pose) + if waypoint is None: + raise ConversionError( + f"Conversion of pose to waypoint for label {label.name} failed") + return LabelSimple(name=label.name, description=label.description, waypoint=waypoint) + + @staticmethod + def simple2label(label_simple: LabelSimple) -> Label: + pose = wp2pose(label_simple.waypoint) + if pose is None: + raise ConversionError( + f"Conversion of waypoint to pose for label {label_simple.name} failed") + return Label(name=label_simple.name, description=label_simple.description, pose=pose) + + +rospy.loginfo("Labels manager ready") + + +if __name__ == '__main__': + rospy.init_node("labels_manager") + try: + labels_manager = LabelsManager() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/opentera_webrtc_ros/scripts/libmapimageconverter.py b/opentera_webrtc_ros/scripts/libmapimageconverter.py new file mode 100755 index 0000000..592b8ef --- /dev/null +++ b/opentera_webrtc_ros/scripts/libmapimageconverter.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 + +import rospy +from tf.transformations import quaternion_from_euler, euler_from_quaternion +from geometry_msgs.msg import PoseStamped +from opentera_webrtc_ros_msgs.msg import Waypoint +from typing import Optional +from map_image_generator.srv import ImageGoalToMapGoal, ImageGoalToMapGoalResponse +# from map_image_generator.srv import MapGoalToImageGoal, MapGoalToImageGoalResponse + + +def convert_pose_to_waypoint(pose: PoseStamped) -> Optional[Waypoint]: + waypoint_pose = _map_goal_to_image_goal_client(pose) + if waypoint_pose is None: + return None + waypoint = Waypoint() + waypoint.x = waypoint_pose.pose.position.x + waypoint.y = waypoint_pose.pose.position.y + waypoint.yaw = euler_from_quaternion(waypoint_pose.pose.orientation)[2] + return waypoint + + +def convert_waypoint_to_pose(waypoint: Waypoint) -> Optional[PoseStamped]: + pose = PoseStamped() + pose.header.frame_id = "map" + pose.pose.position.x = waypoint.x + pose.pose.position.y = waypoint.y + pose.pose.position.z = 0 + yaw = -waypoint.yaw + quaternion = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = quaternion[0] + pose.pose.orientation.y = quaternion[1] + pose.pose.orientation.z = quaternion[2] + pose.pose.orientation.w = quaternion[3] + + return _image_goal_to_map_goal_client(pose) + + +def _map_goal_to_image_goal_client(map_goal: PoseStamped) -> Optional[PoseStamped]: + pass + # rospy.wait_for_service('map_goal_to_image_goal') + # try: + # map_goal_to_image_goal = rospy.ServiceProxy( + # 'map_goal_to_image_goal', MapGoalToImageGoal) + # res: MapGoalToImageGoalResponse = map_goal_to_image_goal( + # map_goal) + # return res.map_goal + # except rospy.ServiceException as e: + # rospy.logwarn("Service call failed: %s" % e) + + +def _image_goal_to_map_goal_client(image_goal: PoseStamped) -> Optional[PoseStamped]: + rospy.wait_for_service('image_goal_to_map_goal') + try: + image_goal_to_map_goal = rospy.ServiceProxy( + 'image_goal_to_map_goal', ImageGoalToMapGoal) + res: ImageGoalToMapGoalResponse = image_goal_to_map_goal( + image_goal) + return res.map_goal + except rospy.ServiceException as e: + rospy.logwarn("Service call failed: %s" % e) diff --git a/opentera_webrtc_ros/scripts/libyamldatabase.py b/opentera_webrtc_ros/scripts/libyamldatabase.py new file mode 100644 index 0000000..c1310e2 --- /dev/null +++ b/opentera_webrtc_ros/scripts/libyamldatabase.py @@ -0,0 +1,87 @@ +#! /usr/bin/env python3 + + +from pathlib import Path +from ruamel.yaml import YAML +from typing import TypeVar, Generic, Generator, Union, Dict + + +T = TypeVar("T") + + +class YamlDatabase(Generic[T]): + def __init__(self, path: Union[Path, str], contained_type: type = None) -> None: + self.path = Path(path).expanduser() + self.path.touch(exist_ok=True) + + self.yaml = YAML() + self.yaml.indent(mapping=2, sequence=4, offset=2) + self.yaml.explicit_start = True + self.yaml.explicit_end = True + if contained_type is not None: + self.yaml.register_class(contained_type) + + self.data = self._load() + + def __getitem__(self, __name: str) -> T: + return self.data[__name] + + def __setitem__(self, __name: str, __value: T) -> None: + self.data[__name] = __value + + def __delitem__(self, __name: str) -> None: + if __name in self.data: + del self.data[__name] + + def __contains__(self, __name: str) -> bool: + return __name in self.data.keys() + + def get(self, name: str) -> T: + if name not in self.data.keys(): + raise IndexError(f"{name} not found in database") + else: + return self.data[name] + + def remove(self, name: str) -> None: + if name not in self.data.keys(): + raise IndexError(f"{name} not found in database") + else: + del self.data[name] + + def add(self, name: str, element: T) -> None: + if name in self.data.keys(): + raise IndexError(f"{name} already exists in database") + else: + self.data[name] = element + + def rename(self, old_name: str, new_name: str) -> None: + if old_name not in self.data.keys(): + raise IndexError(f"{old_name} not found in database") + elif new_name in self.data.keys(): + raise IndexError(f"{new_name} already exists in database") + else: + self.data[new_name] = self.data.pop(old_name) + + def replace(self, name: str, value: T) -> None: + if name not in self.data.keys(): + raise IndexError(f"{name} not found in database") + else: + self.data[name] = value + + def commit(self) -> None: + self.yaml.dump(self.data, self.path) + self.refresh() + + def refresh(self) -> None: + self.data = self._load() + + def clear(self) -> None: + self.path.unlink(missing_ok=True) + self.path.touch() + self.refresh() + + def values(self) -> Generator[T, None, None]: + return (v for v in self.data.values()) + + def _load(self) -> Dict[str, T]: + return self.yaml.load(self.path) or {} diff --git a/opentera_webrtc_ros/setup.py b/opentera_webrtc_ros/setup.py index c85bc5f..8e585f2 100644 --- a/opentera_webrtc_ros/setup.py +++ b/opentera_webrtc_ros/setup.py @@ -1,11 +1,12 @@ #!/usr/bin/env python -from distutils.core import setup +from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['opentera_webrtc_ros'], - package_dir={'': 'src'} - ) + packages=['opentera_webrtc_ros'], + package_dir={'': 'src'}, + +) setup(**d) diff --git a/opentera_webrtc_ros_msgs/CMakeLists.txt b/opentera_webrtc_ros_msgs/CMakeLists.txt index e487834..ced1eaf 100644 --- a/opentera_webrtc_ros_msgs/CMakeLists.txt +++ b/opentera_webrtc_ros_msgs/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + geometry_msgs message_generation sensor_msgs audio_utils @@ -16,6 +17,12 @@ add_message_files( DeviceEvent.msg JoinSessionEvent.msg JoinSessionReplyEvent.msg + LabelArray.msg + LabelEdit.msg + Label.msg + LabelSimple.msg + LabelSimpleArray.msg + LabelSimpleEdit.msg LeaveSessionEvent.msg LogEvent.msg ParticipantEvent.msg @@ -37,10 +44,11 @@ generate_messages( std_msgs sensor_msgs audio_utils + geometry_msgs ) catkin_package( - CATKIN_DEPENDS roscpp rospy std_msgs message_runtime + CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs message_runtime ) include_directories( diff --git a/opentera_webrtc_ros_msgs/msg/Label.msg b/opentera_webrtc_ros_msgs/msg/Label.msg new file mode 100644 index 0000000..1d8c126 --- /dev/null +++ b/opentera_webrtc_ros_msgs/msg/Label.msg @@ -0,0 +1,3 @@ +string name +string description +geometry_msgs/PoseStamped pose diff --git a/opentera_webrtc_ros_msgs/msg/LabelArray.msg b/opentera_webrtc_ros_msgs/msg/LabelArray.msg new file mode 100644 index 0000000..9910fb8 --- /dev/null +++ b/opentera_webrtc_ros_msgs/msg/LabelArray.msg @@ -0,0 +1 @@ +opentera_webrtc_ros_msgs/Label[] labels diff --git a/opentera_webrtc_ros_msgs/msg/LabelEdit.msg b/opentera_webrtc_ros_msgs/msg/LabelEdit.msg new file mode 100644 index 0000000..cad6f2f --- /dev/null +++ b/opentera_webrtc_ros_msgs/msg/LabelEdit.msg @@ -0,0 +1,2 @@ +string current_name +opentera_webrtc_ros_msgs/Label updated diff --git a/opentera_webrtc_ros_msgs/msg/LabelSimple.msg b/opentera_webrtc_ros_msgs/msg/LabelSimple.msg new file mode 100644 index 0000000..3b5be5f --- /dev/null +++ b/opentera_webrtc_ros_msgs/msg/LabelSimple.msg @@ -0,0 +1,3 @@ +string name +string description +opentera_webrtc_ros_msgs/Waypoint waypoint diff --git a/opentera_webrtc_ros_msgs/msg/LabelSimpleArray.msg b/opentera_webrtc_ros_msgs/msg/LabelSimpleArray.msg new file mode 100644 index 0000000..ab24c8b --- /dev/null +++ b/opentera_webrtc_ros_msgs/msg/LabelSimpleArray.msg @@ -0,0 +1 @@ +opentera_webrtc_ros_msgs/LabelSimple[] labels diff --git a/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg b/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg new file mode 100644 index 0000000..54cc9c1 --- /dev/null +++ b/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg @@ -0,0 +1,2 @@ +string current_name +opentera_webrtc_ros_msgs/LabelSimple updated diff --git a/opentera_webrtc_ros_msgs/package.xml b/opentera_webrtc_ros_msgs/package.xml index 90cb253..5a612d3 100644 --- a/opentera_webrtc_ros_msgs/package.xml +++ b/opentera_webrtc_ros_msgs/package.xml @@ -52,13 +52,16 @@ roscpp rospy std_msgs + geometry_msgs message_generation roscpp rospy std_msgs + geometry_msgs roscpp rospy std_msgs + geometry_msgs message_runtime From 4524ef21ddf4753a37213d0b536c5ecdd7a3240e Mon Sep 17 00:00:00 2001 From: philippewarren Date: Mon, 24 Jan 2022 12:15:45 -0500 Subject: [PATCH 3/8] Added label navigation to frontend --- .../drawers/LabelImageDrawer.h | 14 ++- .../src/drawers/LabelImageDrawer.cpp | 44 ++++++--- .../launch/opentera_turtlebot_sim.launch | 4 +- .../opentera-webrtc-teleop-frontend | 2 +- .../include/RosJsonDataHandler.h | 2 + opentera_webrtc_ros/scripts/goal_manager.py | 90 +++++-------------- opentera_webrtc_ros/scripts/labels_manager.py | 25 ++++-- opentera_webrtc_ros/scripts/libnavigation.py | 87 ++++++++++++++++++ .../src/RosJsonDataHandler.cpp | 7 ++ 9 files changed, 186 insertions(+), 89 deletions(-) create mode 100644 opentera_webrtc_ros/scripts/libnavigation.py diff --git a/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h b/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h index 4ddf362..3b505cc 100644 --- a/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h +++ b/map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h @@ -3,10 +3,17 @@ #include "map_image_generator/drawers/ImageDrawer.h" +#include +#include +#include + namespace map_image_generator { class LabelImageDrawer : public ImageDrawer { + ros::Subscriber m_labelArraySubscriber; + opentera_webrtc_ros_msgs::LabelArray::ConstPtr m_lastLabelArray; + public: LabelImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener); @@ -15,8 +22,11 @@ namespace map_image_generator void draw(cv::Mat& image) override; private: - void drawLabel(const geometry_msgs::PoseStamped& label, const std::string& text, - cv::Mat& image, tf::Transform& transform); + void drawLabel(const opentera_webrtc_ros_msgs::Label& label, cv::Mat& image, + tf::Transform& transform); + + void labelArrayCallback( + const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray); }; } #endif diff --git a/map_image_generator/src/drawers/LabelImageDrawer.cpp b/map_image_generator/src/drawers/LabelImageDrawer.cpp index 9354c02..8265134 100644 --- a/map_image_generator/src/drawers/LabelImageDrawer.cpp +++ b/map_image_generator/src/drawers/LabelImageDrawer.cpp @@ -7,23 +7,46 @@ using namespace map_image_generator; LabelImageDrawer::LabelImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle, tf::TransformListener& tfListener) - : ImageDrawer(parameters, nodeHandle, tfListener) + : ImageDrawer(parameters, nodeHandle, tfListener), + m_labelArraySubscriber{m_nodeHandle.subscribe( + "stored_labels", 1, &LabelImageDrawer::labelArrayCallback, this)} { } LabelImageDrawer::~LabelImageDrawer() = default; -void LabelImageDrawer::draw(cv::Mat& image) {} +void LabelImageDrawer::labelArrayCallback( + const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray) +{ + m_lastLabelArray = labelArray; +} + + +void LabelImageDrawer::draw(cv::Mat& image) +{ + if (!m_lastLabelArray) + { + return; + } + + for (const auto& label : m_lastLabelArray->labels) + { + auto tf = getTransformInRef(label.pose.header.frame_id); + if (tf) + { + drawLabel(label, image, *tf); + } + } +} -void LabelImageDrawer::drawLabel(const geometry_msgs::PoseStamped& label, - const std::string& text, cv::Mat& image, - tf::Transform& transform) +void LabelImageDrawer::drawLabel(const opentera_webrtc_ros_msgs::Label& label, + cv::Mat& image, tf::Transform& transform) { const cv::Scalar& color = m_parameters.labelColor(); int size = m_parameters.labelSize(); tf::Pose labelPose; - tf::poseMsgToTF(label.pose, labelPose); + tf::poseMsgToTF(label.pose.pose, labelPose); labelPose = transform * labelPose; adjustTransformForRobotRef(labelPose); double yaw = tf::getYaw(labelPose.getRotation()); @@ -34,8 +57,9 @@ void LabelImageDrawer::drawLabel(const geometry_msgs::PoseStamped& label, int endX = static_cast(startX + size * cos(yaw)); int endY = static_cast(startY + size * sin(yaw)); - cv::circle(image, cv::Point(startX, startY), static_cast(ceil(size / 5.0)), - color, cv::FILLED); - cv::putText(image, text, cv::Point(startX, startY), cv::FONT_HERSHEY_DUPLEX, 0.5, - m_parameters.textColor(), 1); + cv::drawMarker(image, cv::Point(startX, startY), color, cv::MARKER_DIAMOND, + static_cast(ceil(size / 4.0)), + static_cast(ceil(size / 12.0)), cv::FILLED); + cv::putText(image, label.name, cv::Point(startX, startY), cv::FONT_HERSHEY_DUPLEX, + 0.5, m_parameters.textColor(), 1); } diff --git a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch index fb4ab84..b488b78 100644 --- a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch +++ b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch @@ -108,5 +108,7 @@ - + + + diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index 586931a..dedd547 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit 586931a616203e966a6a0f1aee0ece43cd544713 +Subproject commit dedd5475c4b4ec63703d82c70de5cbf5a38597ce diff --git a/opentera_webrtc_ros/include/RosJsonDataHandler.h b/opentera_webrtc_ros/include/RosJsonDataHandler.h index da975c5..01daf06 100644 --- a/opentera_webrtc_ros/include/RosJsonDataHandler.h +++ b/opentera_webrtc_ros/include/RosJsonDataHandler.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -22,6 +23,7 @@ namespace opentera ros::Publisher m_startPub; ros::Publisher m_cmdVelPublisher; ros::Publisher m_waypointsPub; + ros::Publisher m_navigateToLabelPub; float m_linear_multiplier; float m_angular_multiplier; ros::ServiceClient m_dockingClient; diff --git a/opentera_webrtc_ros/scripts/goal_manager.py b/opentera_webrtc_ros/scripts/goal_manager.py index 2f69242..7684368 100755 --- a/opentera_webrtc_ros/scripts/goal_manager.py +++ b/opentera_webrtc_ros/scripts/goal_manager.py @@ -1,26 +1,16 @@ #!/usr/bin/env python3 import rospy -import actionlib -import json -from tf.transformations import quaternion_from_euler -from geometry_msgs.msg import PoseStamped from opentera_webrtc_ros_msgs.msg import WaypointArray -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from std_msgs.msg import Bool, String -from std_srvs.srv import SetBool +from std_msgs.msg import Bool from libmapimageconverter import convert_waypoint_to_pose +from libnavigation import WaypointNavigationClient class GoalManager(): def __init__(self): - - # Global action client used to send waypoints to move_base - self.move_base_client = actionlib.SimpleActionClient( - 'move_base', MoveBaseAction) - self.move_base_client.wait_for_server() - # Make sure no goals are active - self.cancelAllGoalsClient() + self.nav_client = WaypointNavigationClient( + stop_cb=self.__stop_cb, stop_token=self.__stop_token) self.should_stop = False self.pose_goals = [] @@ -28,84 +18,44 @@ def __init__(self): # Subscribers and publishers self.waypoints_sub = rospy.Subscriber( "waypoints", WaypointArray, self.waypoints_cb) - self.stop_sub = rospy.Subscriber("stop", Bool, self.stop_cb) - self.start_sub = rospy.Subscriber("start", Bool, self.start_cb) - self.waypoint_reached_pub = rospy.Publisher( - "waypoint_reached", String, queue_size=1) - self.remove_waypoint_from_image_pub = rospy.Publisher( - "map_image_drawer/remove_goal", PoseStamped, queue_size=10) - self.add_waypoint_to_image_pub = rospy.Publisher( - "map_image_drawer/add_goal", PoseStamped, queue_size=10) + self.start_sub = rospy.Subscriber( + "start", Bool, self.start_cb, queue_size=1) rospy.loginfo("Goal manager ready") + def __stop_token(self, *_, **__): + return self.should_stop + def waypoints_cb(self, msg): for waypoint in msg.waypoints: pose_goal = convert_waypoint_to_pose(waypoint) if pose_goal is not None: pose_goal.pose.position.z = 1 + len(self.pose_goals) - self.add_waypoint_to_image_pub.publish(pose_goal) + self.nav_client.add_to_image(pose_goal) self.pose_goals.append(pose_goal) - def send_goal(self, pose_goal): - goal = MoveBaseGoal() - goal.target_pose = pose_goal - self.move_base_client.send_goal(goal) - self.move_base_client.wait_for_result() - return self.move_base_client.get_state() - - def stop_cb(self, msg): + def __stop_cb(self, _): rospy.loginfo("Stopping") - if msg.data == True: - self.should_stop = True - self.cancelAllGoalsClient() - self.clearGlobalPathClient() - self.pose_goals.clear() + self.should_stop = True + yield + self.pose_goals.clear() + yield def start_cb(self, msg): rospy.loginfo("Starting") if msg.data == True: - self.cancelAllGoalsClient(False) - self.clearGlobalPathClient() + self.nav_client.cancel_all_goals(False) + self.nav_client.clear_global_path() self.should_stop = False for pose_goal in self.pose_goals: - self.send_goal(pose_goal) + self.nav_client.navigate_to_goal( + pose_goal, round(pose_goal.pose.position.z)) if self.should_stop: break - else: - self.publishWaypointReached( - round(pose_goal.pose.position.z), pose_goal) - self.clearGlobalPathClient() + self.nav_client.clear_global_path() self.pose_goals.clear() - def publishWaypointReached(self, i, goal_pose): - waypoint_reached_json_message = { - "type": "waypointReached", "waypointNumber": i} - waypoint_reached_msg = json.dumps(waypoint_reached_json_message) - self.waypoint_reached_pub.publish(waypoint_reached_msg) - self.remove_waypoint_from_image_pub.publish(goal_pose) - - def cancelAllGoalsClient(self, clear_goals=True): - self.move_base_client.cancel_all_goals() - if (clear_goals): - rospy.wait_for_service('map_image_drawer/clear_goals') - try: - image_goal_clear_waypoints = rospy.ServiceProxy( - 'map_image_drawer/clear_goals', SetBool) - res = image_goal_clear_waypoints(True) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) - - def clearGlobalPathClient(self): - rospy.wait_for_service('clear_global_path') - try: - clear_global_path = rospy.ServiceProxy( - 'clear_global_path', SetBool) - clear_global_path(True) - except rospy.ServiceException as e: - rospy.logwarn("Service call failed: %s" % e) - if __name__ == '__main__': rospy.init_node("goal_manager") diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py index 4b797ac..28d395d 100755 --- a/opentera_webrtc_ros/scripts/labels_manager.py +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -3,6 +3,7 @@ from __future__ import annotations import rospy +from pathlib import Path from rospy_message_converter.message_converter import convert_ros_message_to_dictionary as ros2dict from rospy_message_converter.message_converter import convert_dictionary_to_ros_message as dict2ros from opentera_webrtc_ros_msgs.msg import LabelSimple, LabelSimpleArray, LabelSimpleEdit @@ -12,10 +13,8 @@ from geometry_msgs.msg import PoseStamped from libmapimageconverter import convert_waypoint_to_pose as wp2pose from libmapimageconverter import convert_pose_to_waypoint as pose2wp - from libyamldatabase import YamlDatabase -from pathlib import Path -from typing import cast +from libnavigation import WaypointNavigationClient class ConversionError(Exception): @@ -59,6 +58,8 @@ def __init__(self) -> None: "remove_label_by_name", String, self.remove_label_by_name_callback, queue_size=1) self.edit_label_simple_sub = rospy.Subscriber( "edit_label_simple", LabelSimpleEdit, self.edit_label_simple_callback, queue_size=1) + self.navigate_to_label_sub = rospy.Subscriber( + "navigate_to_label", String, self.navigate_to_label_callback, queue_size=1) self.stored_labels_pub = rospy.Publisher( "stored_labels", LabelArray, queue_size=1) @@ -71,6 +72,8 @@ def __init__(self) -> None: self.pub_timer = rospy.Timer(rospy.Duration( 1), self.publish_stored_labels_simple) + self.nav_client = WaypointNavigationClient() + rospy.loginfo("Labels manager initialized") def publish_stored_labels_simple(self, _: rospy.timer.TimerEvent) -> None: @@ -94,14 +97,26 @@ def remove_label_by_name_callback(self, msg: String) -> None: def edit_label_simple_callback(self, msg: LabelSimpleEdit) -> None: try: - self.db.replace(msg.current_name, LabelData( - self.simple2label(msg.updated))) if msg.current_name != msg.updated.name: self.db.rename(msg.current_name, msg.updated.name) + + self.db.replace(msg.updated.name, LabelData( + self.simple2label(msg.updated))) + self.db.commit() except (IndexError, ConversionError) as e: rospy.logerr(f"Editing label in database failed: {e}") + def navigate_to_label_callback(self, msg: String) -> None: + if not msg.data in self.db: + rospy.logerr( + f"Navigation to label failed: Label [{msg.data}] not in database") + return + + label = self.db[msg.data].label + self.nav_client.add_to_image(label.pose) + self.nav_client.navigate_to_goal(label.pose, 1) + @staticmethod def label2simple(label: Label) -> LabelSimple: waypoint = pose2wp(label.pose) diff --git a/opentera_webrtc_ros/scripts/libnavigation.py b/opentera_webrtc_ros/scripts/libnavigation.py new file mode 100644 index 0000000..ecae151 --- /dev/null +++ b/opentera_webrtc_ros/scripts/libnavigation.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +import rospy +import actionlib +import json +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +from std_srvs.srv import SetBool +from std_msgs.msg import Bool, String +from geometry_msgs.msg import PoseStamped +from typing import Optional, Generator, Callable + + +class WaypointNavigationClient: + def __init__(self, stop_cb: Optional[Callable[..., Generator]] = None, stop_token=None) -> None: + self.move_base_client = actionlib.SimpleActionClient( + 'move_base', MoveBaseAction) + self.move_base_client.wait_for_server() + + self.stop_sub = rospy.Subscriber( + "stop", Bool, self._stop_callback, queue_size=1) + + self.waypoint_reached_pub = rospy.Publisher( + "waypoint_reached", String, queue_size=1) + self.remove_waypoint_from_image_pub = rospy.Publisher( + "map_image_drawer/remove_goal", PoseStamped, queue_size=10) + self.add_waypoint_to_image_pub = rospy.Publisher( + "map_image_drawer/add_goal", PoseStamped, queue_size=10) + + self.stop_cb = stop_cb or WaypointNavigationClient.__noop + self.stop_token = stop_token or WaypointNavigationClient.__false + + self.cancel_all_goals() + + @staticmethod + def __noop(*_, **__) -> Generator: + while True: + yield + + @staticmethod + def __false(*_, **__): return False + + def add_to_image(self, pose_goal): + self.add_waypoint_to_image_pub.publish(pose_goal) + + def navigate_to_goal(self, pose_goal, reached_index): + goal = MoveBaseGoal() + goal.target_pose = pose_goal + self.move_base_client.send_goal(goal) + self.move_base_client.wait_for_result() + state = self.move_base_client.get_state() + if not self.stop_token(state): + self._publish_waypoint_reached(reached_index, pose_goal) + + def cancel_all_goals(self, clear_goals=True): + self.move_base_client.cancel_all_goals() + if (clear_goals): + rospy.wait_for_service('map_image_drawer/clear_goals') + try: + image_goal_clear_waypoints = rospy.ServiceProxy( + 'map_image_drawer/clear_goals', SetBool) + image_goal_clear_waypoints(True) + except rospy.ServiceException as e: + rospy.logwarn("Service call failed: %s" % e) + + def clear_global_path(self): + rospy.wait_for_service('clear_global_path') + try: + clear_global_path = rospy.ServiceProxy( + 'clear_global_path', SetBool) + clear_global_path(True) + except rospy.ServiceException as e: + rospy.logwarn("Service call failed: %s" % e) + + def _publish_waypoint_reached(self, waypoint_index, goal_pose): + waypoint_reached_json_message = { + "type": "waypointReached", "waypointNumber": waypoint_index} + waypoint_reached_msg = json.dumps(waypoint_reached_json_message) + self.waypoint_reached_pub.publish(waypoint_reached_msg) + self.remove_waypoint_from_image_pub.publish(goal_pose) + + def _stop_callback(self, msg): + if msg.data == True: + cb = self.stop_cb(msg) + next(cb) + self.cancel_all_goals() + self.clear_global_path() + next(cb) diff --git a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp index fd6ea92..fbbea81 100644 --- a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp +++ b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp @@ -14,6 +14,7 @@ RosJsonDataHandler::RosJsonDataHandler(const ros::NodeHandle& nh, m_cmdVelPublisher = m_nh.advertise("cmd_vel", 1); m_waypointsPub = m_nh.advertise("waypoints", 1); + m_navigateToLabelPub = m_nh.advertise("navigate_to_label", 1); m_webrtcDataSubscriber = m_nh.subscribe("webrtc_data", 1, &RosJsonDataHandler::onWebRTCDataReceived, this); @@ -137,6 +138,12 @@ void RosJsonDataHandler::onWebRTCDataReceived( srv.response.message.c_str()); } } + else if (serializedData["type"] == "goToLabel") + { + std_msgs::String msg; + msg.data = serializedData["label"]; + m_navigateToLabelPub.publish(msg); + } } /** From 82c1e074269e7643885884210642ff622019f3e7 Mon Sep 17 00:00:00 2001 From: philippewarren Date: Mon, 24 Jan 2022 14:03:54 -0500 Subject: [PATCH 4/8] Added sync between labels stored by label_manager and frontend --- .../launch/opentera_turtlebot_sim.launch | 1 + .../opentera-webrtc-teleop-frontend | 2 +- opentera_webrtc_ros/README.md | 26 +++++++++++++++++-- opentera_webrtc_ros/scripts/labels_manager.py | 18 ++++++++++--- 4 files changed, 41 insertions(+), 6 deletions(-) diff --git a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch index b488b78..51ab310 100644 --- a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch +++ b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch @@ -110,5 +110,6 @@ + diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index dedd547..f3166c4 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit dedd5475c4b4ec63703d82c70de5cbf5a38597ce +Subproject commit f3166c4e7819c14462244dc8035914f8eae8b6d7 diff --git a/opentera_webrtc_ros/README.md b/opentera_webrtc_ros/README.md index d4c6677..6f50f9c 100644 --- a/opentera_webrtc_ros/README.md +++ b/opentera_webrtc_ros/README.md @@ -120,6 +120,23 @@ Implement a ROS node that dispatch received JSON messages and forward them on th For usage exemple look at [ros_json_data_handler.launch](launch/ros_json_data_handler.launch). +# libnavigation + +## Description + +Shared library for `goal_manager` and `label_manager` to send navigation commands to `move_base` as well as navigation waypoints to `map_image_generator`. +The library is embedded in both nodes, a single instance is not shared. + +## Subscribed topics + +- stop (`std_msgs/Bool`): Signal to cancel all move_base goals. + +## Published topics + +- waypoint_reached (`std_msgs/String`): String of a JSON message containing the ID of the waypoint that has been reached. Used by the frontend to determine when the trajectory has been completed. +- map_image_drawer/remove_goal: (`geometry_msgs/PoseStamped`): Removes a waypoint from the map image +- map_image_drawer/add_goal: (`geometry_msgs/PoseStamped`): Adds a waypoint to the map image + # goal_manager ## Description @@ -129,11 +146,12 @@ Manages multiple waypoints received by the frontend and sends them to move_base ## Subscribed topics - waypoints (`opentera_webrtc_ros_msgs/Waypoints`): Array of image coordinate waypoints received from the frontend. -- stop (`std_msgs/Bool`): Signal to cancel all move_base goals. +- start (`std_msgs/Bool`): Signal to start navigating sequentially to the previously received waypoints. +- All subscribed topics of `libnavigation` ## Published topics -- waypoint_reached (`std_msgs/String`): String of a JSON message containing the ID of the waypoint that has been reached. Used by the frontend to determine when the trajectory has been completed. +- All published topics of `libnavigation` # labels_manager @@ -151,8 +169,12 @@ This node relies on a service provided by the `map_image_generator` package to c - add_label_simple (`opentera_webrtc_ros_msgs/LabelSimple`): Label received from the frontend. - remove_label_by_name (`std_msgs/String`): Remove a label by its name - edit_label_simple (`opentera_webrtc_ros_msgs/LabelSimpleEdit`): Rename or move a label using frontend map coordinates +- navigate_to_label (`std_msgs/String`): Navigate to a label by its name +- All subscribed topics of `libnavigation` ## Published topics - stored_labels (`opentera_webrtc_ros_msgs/LabelArray`): Array of labels currently stored +- stored_labels_names (`std_msgs/String`): JSON message with an array of string which are the stored labels names. Used by the frontend to display a list of labels. +- All published topics of `libnavigation` diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py index 28d395d..16889b8 100755 --- a/opentera_webrtc_ros/scripts/labels_manager.py +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -3,6 +3,7 @@ from __future__ import annotations import rospy +import json from pathlib import Path from rospy_message_converter.message_converter import convert_ros_message_to_dictionary as ros2dict from rospy_message_converter.message_converter import convert_dictionary_to_ros_message as dict2ros @@ -63,20 +64,31 @@ def __init__(self) -> None: self.stored_labels_pub = rospy.Publisher( "stored_labels", LabelArray, queue_size=1) + self.stored_labels_names_pub = rospy.Publisher( + "stored_labels_names", String, queue_size=1) self.database_path: str = rospy.get_param( "~database_path", "~/.ros/labels.yaml") self.db: YamlDatabase[LabelData] = YamlDatabase( Path(self.database_path), LabelData) - self.pub_timer = rospy.Timer(rospy.Duration( - 1), self.publish_stored_labels_simple) + self.pub_timer_stored_labels = rospy.Timer(rospy.Duration( + 1), self.publish_stored_labels) + self.pub_timer_stored_labels_names = rospy.Timer(rospy.Duration( + 1), self.publish_stored_labels_names) self.nav_client = WaypointNavigationClient() rospy.loginfo("Labels manager initialized") - def publish_stored_labels_simple(self, _: rospy.timer.TimerEvent) -> None: + def publish_stored_labels_names(self, _: rospy.timer.TimerEvent) -> None: + labels_names = list(e.label.name for e in self.db.values()) + labels_names_json_message = { + "type": "labels", "labels": labels_names} + labels_names_msg = json.dumps(labels_names_json_message) + self.stored_labels_names_pub.publish(labels_names_msg) + + def publish_stored_labels(self, _: rospy.timer.TimerEvent) -> None: labels = tuple(e.label for e in self.db.values()) self.stored_labels_pub.publish(labels) From 4e6f65258af5d24249a498f856b0313c4ff121b9 Mon Sep 17 00:00:00 2001 From: philippewarren Date: Tue, 25 Jan 2022 11:01:43 -0500 Subject: [PATCH 5/8] Added label addition and move to frontend --- .../launch/opentera_turtlebot_sim.launch | 2 +- .../opentera-webrtc-teleop-frontend | 2 +- .../include/RosJsonDataHandler.h | 11 +++- opentera_webrtc_ros/scripts/labels_manager.py | 21 +++---- .../src/RosJsonDataHandler.cpp | 57 ++++++++++++++++--- 5 files changed, 72 insertions(+), 21 deletions(-) diff --git a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch index 51ab310..cdd6883 100644 --- a/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch +++ b/opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch @@ -110,6 +110,6 @@ - + diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index f3166c4..96c7f2c 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit f3166c4e7819c14462244dc8035914f8eae8b6d7 +Subproject commit 96c7f2cceccdae70f675ca727e6f6c470c1277f9 diff --git a/opentera_webrtc_ros/include/RosJsonDataHandler.h b/opentera_webrtc_ros/include/RosJsonDataHandler.h index 01daf06..f7b863b 100644 --- a/opentera_webrtc_ros/include/RosJsonDataHandler.h +++ b/opentera_webrtc_ros/include/RosJsonDataHandler.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include #include @@ -24,6 +26,9 @@ namespace opentera ros::Publisher m_cmdVelPublisher; ros::Publisher m_waypointsPub; ros::Publisher m_navigateToLabelPub; + ros::Publisher m_removeLabelPub; + ros::Publisher m_addLabelPub; + ros::Publisher m_editLabelPub; float m_linear_multiplier; float m_angular_multiplier; ros::ServiceClient m_dockingClient; @@ -44,7 +49,11 @@ namespace opentera RosJsonDataHandler(const ros::NodeHandle& nh, const ros::NodeHandle& p_nh); virtual ~RosJsonDataHandler(); - void run(); + static void run(); + + private: + static opentera_webrtc_ros_msgs::Waypoint + getWpFromData(const nlohmann::json& data); }; } diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py index 16889b8..20c07d0 100755 --- a/opentera_webrtc_ros/scripts/labels_manager.py +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -64,8 +64,8 @@ def __init__(self) -> None: self.stored_labels_pub = rospy.Publisher( "stored_labels", LabelArray, queue_size=1) - self.stored_labels_names_pub = rospy.Publisher( - "stored_labels_names", String, queue_size=1) + self.stored_labels_text_pub = rospy.Publisher( + "stored_labels_text", String, queue_size=1) self.database_path: str = rospy.get_param( "~database_path", "~/.ros/labels.yaml") @@ -74,19 +74,20 @@ def __init__(self) -> None: self.pub_timer_stored_labels = rospy.Timer(rospy.Duration( 1), self.publish_stored_labels) - self.pub_timer_stored_labels_names = rospy.Timer(rospy.Duration( - 1), self.publish_stored_labels_names) + self.pub_timer_stored_labels_text = rospy.Timer(rospy.Duration( + 1), self.publish_stored_labels_text) self.nav_client = WaypointNavigationClient() rospy.loginfo("Labels manager initialized") - def publish_stored_labels_names(self, _: rospy.timer.TimerEvent) -> None: - labels_names = list(e.label.name for e in self.db.values()) - labels_names_json_message = { - "type": "labels", "labels": labels_names} - labels_names_msg = json.dumps(labels_names_json_message) - self.stored_labels_names_pub.publish(labels_names_msg) + def publish_stored_labels_text(self, _: rospy.timer.TimerEvent) -> None: + labels_text = [ + {"name": e.label.name, "description": e.label.description} for e in self.db.values()] + labels_text_json_message = { + "type": "labels", "labels": labels_text} + labels_text_msg = json.dumps(labels_text_json_message) + self.stored_labels_text_pub.publish(labels_text_msg) def publish_stored_labels(self, _: rospy.timer.TimerEvent) -> None: labels = tuple(e.label for e in self.db.values()) diff --git a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp index fbbea81..755a2e5 100644 --- a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp +++ b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp @@ -15,6 +15,11 @@ RosJsonDataHandler::RosJsonDataHandler(const ros::NodeHandle& nh, m_waypointsPub = m_nh.advertise("waypoints", 1); m_navigateToLabelPub = m_nh.advertise("navigate_to_label", 1); + m_removeLabelPub = m_nh.advertise("remove_label_by_name", 1); + m_addLabelPub = + m_nh.advertise("add_label_simple", 1); + m_editLabelPub = + m_nh.advertise("edit_label_simple", 1); m_webrtcDataSubscriber = m_nh.subscribe("webrtc_data", 1, &RosJsonDataHandler::onWebRTCDataReceived, this); @@ -34,6 +39,17 @@ RosJsonDataHandler::RosJsonDataHandler(const ros::NodeHandle& nh, RosJsonDataHandler::~RosJsonDataHandler() = default; +opentera_webrtc_ros_msgs::Waypoint +RosJsonDataHandler::getWpFromData(const nlohmann::json& data) +{ + opentera_webrtc_ros_msgs::Waypoint wp; + wp.x = static_cast(data["coordinate"]["x"]); + wp.y = static_cast(data["coordinate"]["y"]); + wp.yaw = + static_cast(static_cast(data["coordinate"]["yaw"]) * M_PI / 180); + return wp; +} + void RosJsonDataHandler::onWebRTCDataReceived( const ros::MessageEvent& event) { @@ -66,15 +82,9 @@ void RosJsonDataHandler::onWebRTCDataReceived( else if (serializedData["type"] == "waypointArray") { opentera_webrtc_ros_msgs::WaypointArray wp_array; - for (auto waypoint : serializedData["array"]) + for (const auto& waypoint : serializedData["array"]) { - // Received waypoints are in pixel coordinates in the image frame - opentera_webrtc_ros_msgs::Waypoint wp; - wp.x = static_cast(waypoint["coordinate"]["x"]); - wp.y = static_cast(waypoint["coordinate"]["y"]); - wp.yaw = static_cast(static_cast(waypoint["coordinate"]["yaw"]) - * M_PI / 180); - wp_array.waypoints.push_back(wp); + wp_array.waypoints.push_back(getWpFromData(waypoint)); } m_waypointsPub.publish(wp_array); } @@ -144,6 +154,37 @@ void RosJsonDataHandler::onWebRTCDataReceived( msg.data = serializedData["label"]; m_navigateToLabelPub.publish(msg); } + else if (serializedData["type"] == "removeLabel") + { + std_msgs::String msg; + msg.data = serializedData["label"]; + m_removeLabelPub.publish(msg); + } + else if (serializedData["type"] == "addLabel") + { + const auto& data = serializedData["label"]; + + opentera_webrtc_ros_msgs::LabelSimple label; + + label.name = data["name"]; + label.description = data["description"]; + label.waypoint = getWpFromData(data); + + m_addLabelPub.publish(label); + } + else if (serializedData["type"] == "editLabel") + { + const auto& data = serializedData["newLabel"]; + + opentera_webrtc_ros_msgs::LabelSimpleEdit labelEdit; + + labelEdit.current_name = serializedData["currentLabel"]; + labelEdit.updated.name = data["name"]; + labelEdit.updated.description = data["description"]; + labelEdit.updated.waypoint = getWpFromData(data); + + m_editLabelPub.publish(labelEdit); + } } /** From 03426ce2c055fea3ba07ba3dde3d5230e513ca69 Mon Sep 17 00:00:00 2001 From: philippewarren Date: Tue, 25 Jan 2022 12:37:25 -0500 Subject: [PATCH 6/8] Added label editing to frontend --- opentera_webrtc_demos/opentera-webrtc-teleop-frontend | 2 +- opentera_webrtc_ros/README.md | 6 ++---- opentera_webrtc_ros/scripts/labels_manager.py | 6 ++++-- opentera_webrtc_ros/scripts/libnavigation.py | 5 ++++- opentera_webrtc_ros/src/RosJsonDataHandler.cpp | 11 ++++++++++- opentera_webrtc_ros_msgs/msg/LabelEdit.msg | 1 + opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg | 1 + 7 files changed, 23 insertions(+), 9 deletions(-) diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index 96c7f2c..21c55b4 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit 96c7f2cceccdae70f675ca727e6f6c470c1277f9 +Subproject commit 21c55b41ed91a82876dfd4369b36595425a79ed5 diff --git a/opentera_webrtc_ros/README.md b/opentera_webrtc_ros/README.md index 6f50f9c..e46e990 100644 --- a/opentera_webrtc_ros/README.md +++ b/opentera_webrtc_ros/README.md @@ -161,8 +161,7 @@ Manages labels. The stored labels are dependent on the `map` transform and the database needs to be cleaned if the map changes. A label represents a name grouped with an associated pose and a description. This node relies on a service provided by the `map_image_generator` package to convert the labels from image coordinates to map coordinates. - - +This node can also send a label as a goal to `move_base` by its name. ## Subscribed topics @@ -175,6 +174,5 @@ This node relies on a service provided by the `map_image_generator` package to c ## Published topics - stored_labels (`opentera_webrtc_ros_msgs/LabelArray`): Array of labels currently stored -- stored_labels_names (`std_msgs/String`): JSON message with an array of string which are the stored labels names. Used by the frontend to display a list of labels. +- stored_labels_text (`std_msgs/String`): JSON message with an array of maps `{name: string, description: string}` labels names and descriptions. Used by the frontend to display a list of labels. - All published topics of `libnavigation` - diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py index 20c07d0..0a0dfcd 100755 --- a/opentera_webrtc_ros/scripts/labels_manager.py +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -113,8 +113,10 @@ def edit_label_simple_callback(self, msg: LabelSimpleEdit) -> None: if msg.current_name != msg.updated.name: self.db.rename(msg.current_name, msg.updated.name) - self.db.replace(msg.updated.name, LabelData( - self.simple2label(msg.updated))) + updated = self.simple2label(msg.updated) + if msg.ignore_waypoint is True: + updated.pose = self.db[msg.updated.name].label.pose + self.db.replace(msg.updated.name, LabelData(updated)) self.db.commit() except (IndexError, ConversionError) as e: diff --git a/opentera_webrtc_ros/scripts/libnavigation.py b/opentera_webrtc_ros/scripts/libnavigation.py index ecae151..57a2a47 100644 --- a/opentera_webrtc_ros/scripts/libnavigation.py +++ b/opentera_webrtc_ros/scripts/libnavigation.py @@ -28,6 +28,7 @@ def __init__(self, stop_cb: Optional[Callable[..., Generator]] = None, stop_toke self.stop_cb = stop_cb or WaypointNavigationClient.__noop self.stop_token = stop_token or WaypointNavigationClient.__false + self.stop = False self.cancel_all_goals() @@ -45,10 +46,11 @@ def add_to_image(self, pose_goal): def navigate_to_goal(self, pose_goal, reached_index): goal = MoveBaseGoal() goal.target_pose = pose_goal + self.stop = False self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result() state = self.move_base_client.get_state() - if not self.stop_token(state): + if not self.stop and not self.stop_token(state): self._publish_waypoint_reached(reached_index, pose_goal) def cancel_all_goals(self, clear_goals=True): @@ -80,6 +82,7 @@ def _publish_waypoint_reached(self, waypoint_index, goal_pose): def _stop_callback(self, msg): if msg.data == True: + self.stop = True cb = self.stop_cb(msg) next(cb) self.cancel_all_goals() diff --git a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp index 755a2e5..5cae686 100644 --- a/opentera_webrtc_ros/src/RosJsonDataHandler.cpp +++ b/opentera_webrtc_ros/src/RosJsonDataHandler.cpp @@ -181,7 +181,16 @@ void RosJsonDataHandler::onWebRTCDataReceived( labelEdit.current_name = serializedData["currentLabel"]; labelEdit.updated.name = data["name"]; labelEdit.updated.description = data["description"]; - labelEdit.updated.waypoint = getWpFromData(data); + if (data["coordinate"].is_null()) + { + labelEdit.ignore_waypoint = true; + labelEdit.updated.waypoint = opentera_webrtc_ros_msgs::Waypoint(); + } + else + { + labelEdit.ignore_waypoint = false; + labelEdit.updated.waypoint = getWpFromData(data); + } m_editLabelPub.publish(labelEdit); } diff --git a/opentera_webrtc_ros_msgs/msg/LabelEdit.msg b/opentera_webrtc_ros_msgs/msg/LabelEdit.msg index cad6f2f..f8ccdaa 100644 --- a/opentera_webrtc_ros_msgs/msg/LabelEdit.msg +++ b/opentera_webrtc_ros_msgs/msg/LabelEdit.msg @@ -1,2 +1,3 @@ string current_name opentera_webrtc_ros_msgs/Label updated +bool ignore_pose diff --git a/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg b/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg index 54cc9c1..b666b52 100644 --- a/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg +++ b/opentera_webrtc_ros_msgs/msg/LabelSimpleEdit.msg @@ -1,2 +1,3 @@ string current_name opentera_webrtc_ros_msgs/LabelSimple updated +bool ignore_waypoint From 9ff640aef4ea36712fcd664fd655331b81547dc0 Mon Sep 17 00:00:00 2001 From: philippewarren Date: Tue, 25 Jan 2022 12:57:59 -0500 Subject: [PATCH 7/8] Remove unrequired rependency --- opentera_webrtc_ros/scripts/labels_manager.py | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py index 0a0dfcd..4a36758 100755 --- a/opentera_webrtc_ros/scripts/labels_manager.py +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -5,8 +5,6 @@ import rospy import json from pathlib import Path -from rospy_message_converter.message_converter import convert_ros_message_to_dictionary as ros2dict -from rospy_message_converter.message_converter import convert_dictionary_to_ros_message as dict2ros from opentera_webrtc_ros_msgs.msg import LabelSimple, LabelSimpleArray, LabelSimpleEdit from opentera_webrtc_ros_msgs.msg import Label, LabelArray, LabelEdit from opentera_webrtc_ros_msgs.msg import Waypoint @@ -26,7 +24,28 @@ class LabelData: yaml_tag = "!label" def __init__(self, label: Label) -> None: - self.data = ros2dict(label) + self.data = { + "name": label.name, + "description": label.description, + "pose": { + "header": { + "frame_id": label.pose.header.frame_id, + }, + "pose": { + "position": { + "x": label.pose.pose.position.x, + "y": label.pose.pose.position.y, + "z": label.pose.pose.position.z, + }, + "orientation": { + "x": label.pose.pose.orientation.x, + "y": label.pose.pose.orientation.y, + "z": label.pose.pose.orientation.z, + "w": label.pose.pose.orientation.w, + }, + }, + }, + } @property def label(self) -> Label: From d754e3a6ee1e36e65c0dd2f734562bb5f1ffe3d7 Mon Sep 17 00:00:00 2001 From: philippewarren Date: Fri, 28 Jan 2022 14:49:08 -0500 Subject: [PATCH 8/8] Updated dependency --- opentera_webrtc_demos/opentera-webrtc-teleop-frontend | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index 21c55b4..6dd2516 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit 21c55b41ed91a82876dfd4369b36595425a79ed5 +Subproject commit 6dd2516791daed67779b7ddebea793eebf594cb3