Skip to content

Commit

Permalink
Backport #208 (#210)
Browse files Browse the repository at this point in the history 
* Support fleet-level task (#201)

* update task request to include fleet_name for fleet-level task

Signed-off-by: Charly Wu <[email protected]>

* added DCO commit.

Signed-off-by: Charly Wu <[email protected]>

* simplify logic for populating request['fleet_name']

Signed-off-by: Charly Wu <[email protected]>

* remove extra whitespaces.

Signed-off-by: Charly Wu <[email protected]>

* remove trailing whitespace.

Signed-off-by: Charly Wu <[email protected]>

---------

Signed-off-by: Charly Wu <[email protected]>
Co-authored-by: Yadunund <[email protected]>

* Update CI to rolling on main (#208)

* Update CI to rolling

Signed-off-by: Yadunund <[email protected]>

* Address all flake8 and pep257 errors

Signed-off-by: Yadunund <[email protected]>

---------

Signed-off-by: Yadunund <[email protected]>

* Set CI to iron

Signed-off-by: Yadunund <[email protected]>

---------

Signed-off-by: Charly Wu <[email protected]>
Signed-off-by: Yadunund <[email protected]>
Co-authored-by: cwrx777 <[email protected]>
  • Loading branch information
@Yadunund @cwrx777
Yadunund and cwrx777 authored Jan 29, 2024
1 parent 95a02e6 commit 54b48ad
Show file tree
Hide file tree
Showing 29 changed files with 1,614 additions and 1,238 deletions.
67 changes: 19 additions & 48 deletions .github/workflows/build.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,53 +1,24 @@
name: build
on:
pull_request:
schedule:
- cron: '0 0 * * *'
workflow_dispatch:

jobs:
build:
runs-on: ubuntu-latest
strategy:
matrix:
docker_image: ['ros:humble-ros-base']
container:
image: ${{ matrix.docker_image }}
steps:
- name: non-ros-deps
run: |
sudo apt update
sudo apt install -y wget
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add -
sudo apt update && sudo apt install \
git cmake python3-vcstool curl \
clang clang-tools lld \
python3-shapely python3-yaml python3-requests \
python3-pip -y
pip3 install flask-socketio fastapi uvicorn
- name: create-ws
run: |
mkdir -p rmf_demos_ws/src
- name: checkout # This will check out the current rmf_demos branch for build tests
uses: actions/checkout@v2
with:
path: rmf_demos_ws/src/demonstrations/rmf_demos/
- name: workspace
run: |
cd rmf_demos_ws
wget https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos
vcs import src < rmf.repos --skip-existing
- name: ros-deps
run: |
export DEBIAN_FRONTEND=noninteractive
colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
colcon mixin update default
cd rmf_demos_ws
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro humble -yr
- name: build
shell: bash
run: |
cd rmf_demos_ws
source /opt/ros/humble/setup.bash
export CXX=clang++
export CX=clang
colcon build --mixin lld --cmake-args -DNO_DOWNLOAD_MODELS=True
build_and_test:
name: rmf_demos
uses: open-rmf/rmf_ci_templates/.github/workflows/reusable_build.yaml@main
with:
packages: |
rmf_demos
rmf_demos_assets
rmf_demos_bridges
rmf_demos_dashboard_resources
rmf_demos_fleet_adapter
rmf_demos_gz
rmf_demos_gz_classic
rmf_demos_maps
rmf_demos_panel
rmf_demos_tasks
dist-matrix: '[{"ros_distribution": "iron", "ubuntu_distribution": "jammy"}]'
14 changes: 3 additions & 11 deletions .github/workflows/style.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,23 +8,15 @@ jobs:
runs-on: ubuntu-latest
strategy:
matrix:
docker_image: ['ros:humble-ros-base']
docker_image: ['ros:iron-ros-base']
container:
image: ${{ matrix.docker_image }}
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v3
- name: deps
shell: bash
run: |
sudo apt-get update
sudo apt-get install wget pycodestyle
# Note: Enable this if only cpp files are available for style check
# - name: rmf_uncrustify
# shell: bash
# run: |
# wget https://raw.githubusercontent.com/open-rmf/rmf_utils/master/rmf_utils/test/format/rmf_code_style.cfg
# source /opt/ros/foxy/setup.bash
# ament_uncrustify -c rmf_code_style.cfg
sudo apt update && sudo apt install pycodestyle
- name: pycodestyle
shell: bash
run: |
Expand Down
210 changes: 112 additions & 98 deletions rmf_demos_bridges/rmf_demos_bridges/fleet_robotmanager_mqtt_bridge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,96 +13,112 @@
# limitations under the License.


from flask_cors import CORS
from flask import Flask, request, jsonify
import rclpy
import rclpy.executors
import threading
import sys
import json
import copy
import argparse
from collections import OrderedDict
from rosidl_runtime_py import message_to_ordereddict
from pyproj import Transformer
import copy
import json
import sys
import threading

from flask import Flask
from flask import jsonify
from flask import request
from flask_cors import CORS
from flask_socketio import disconnect
from flask_socketio import emit
from flask_socketio import SocketIO
import paho.mqtt.client as mqtt
from pyproj import Transformer
import rclpy
import rclpy.executors
from rclpy.node import Node
from flask_socketio import SocketIO, emit, disconnect

from rclpy.qos import QoSProfile
from rclpy.qos import QoSHistoryPolicy as History
from rclpy.qos import QoSDurabilityPolicy as Durability
from rclpy.qos import QoSHistoryPolicy as History
from rclpy.qos import QoSProfile
from rclpy.qos import QoSReliabilityPolicy as Reliability

from rmf_fleet_msgs.msg import FleetState, RobotState
import paho.mqtt.client as mqtt
from rmf_fleet_msgs.msg import FleetState
from rmf_fleet_msgs.msg import RobotState
from rosidl_runtime_py import message_to_ordereddict

SUPPORTED_GPS_FRAMES = ['svy21']

ROBOT_ID_TO_AUTHKEY_MAP = {
"deliveryRobot_1": "00000000-0000-0000-0000-000000000001",
"deliveryRobot_2": "00000000-0000-0000-0000-000000000002",
"deliveryRobot_3": "00000000-0000-0000-0000-000000000003",
'deliveryRobot_1': '00000000-0000-0000-0000-000000000001',
'deliveryRobot_2': '00000000-0000-0000-0000-000000000002',
'deliveryRobot_3': '00000000-0000-0000-0000-000000000003',
}


ROBOTMANAGER_GPS_DEFINITION = {
"batteryPct": 100.0,
"mapPose": {
"x": 1076.0,
"y": 456.0,
"z": 0.0,
"heading": 0.0
'batteryPct': 100.0,
'mapPose': {'x': 1076.0, 'y': 456.0, 'z': 0.0, 'heading': 0.0},
'globalPose': {
'lat': 1.30032,
'lng': 103.78063,
'alt': 0.0,
'heading': 0.0,
},
"globalPose": {
"lat": 1.30032,
"lng": 103.78063,
"alt": 0.0,
"heading": 0.0
},
"state": 3
'state': 3,
}


class FleetRobotManagerMQTTBridge(Node):
def __init__(self, argv=sys.argv):
parser = argparse.ArgumentParser()
parser.add_argument('-t', '--robot_state_topic',
required=False,
type=str,
default='/robot_state',
help='Topic to listen on for Robot States')
parser.add_argument('-f', '--filter_fleet',
required=False,
type=str,
help='Only listen to this fleet.')
parser.add_argument('-g', '--mqtt_base_topic',
required=False,
type=str,
default="/robot/status/",
help='MQTT base topic to publish GPS.')
parser.add_argument('-x', '--offset_x',
required=False,
type=float,
default=0.0,
help='X offset for simulations.')
parser.add_argument('-y', '--offset_y',
required=False,
type=float,
default=0.0,
help='Y offset for simulations.')
parser.add_argument('-m', '--mqtt_server',
required=False,
type=str,
default="localhost",
help='MQTT server url')
parser.add_argument(
'-t',
'--robot_state_topic',
required=False,
type=str,
default='/robot_state',
help='Topic to listen on for Robot States',
)
parser.add_argument(
'-f',
'--filter_fleet',
required=False,
type=str,
help='Only listen to this fleet.',
)
parser.add_argument(
'-g',
'--mqtt_base_topic',
required=False,
type=str,
default='/robot/status/',
help='MQTT base topic to publish GPS.',
)
parser.add_argument(
'-x',
'--offset_x',
required=False,
type=float,
default=0.0,
help='X offset for simulations.',
)
parser.add_argument(
'-y',
'--offset_y',
required=False,
type=float,
default=0.0,
help='Y offset for simulations.',
)
parser.add_argument(
'-m',
'--mqtt_server',
required=False,
type=str,
default='localhost',
help='MQTT server url',
)

self.args = parser.parse_args(argv[1:])

super().__init__(f"fleet_robotmanager_mqtt_bridge")
super().__init__(f'fleet_robotmanager_mqtt_bridge')

self._init_pubsub()
self.mqtt_pubs = {} # Map of robot names to their mqtt Client
self.mqtt_pubs = {} # Map of robot names to their mqtt Client
self._init_mqtt()
self._init_gps_conversion_tools('svy21')

Expand All @@ -113,14 +129,12 @@ def robot_state_callback(self, msg: RobotState):
return
robot = msg
if robot.name not in ROBOT_ID_TO_AUTHKEY_MAP.keys():
print(
f"Skipping {robot.name} as it does not have auth key")
print(f'Skipping {robot.name} as it does not have auth key')
else:
json = self._robot_state_to_gps_json(robot)
rbmgr_uuid = ROBOT_ID_TO_AUTHKEY_MAP[robot.name]
self.mqtt_pubs[robot.name].publish(
self.args.mqtt_base_topic + rbmgr_uuid,
json
self.args.mqtt_base_topic + rbmgr_uuid, json
)

except Exception as e:
Expand All @@ -131,61 +145,62 @@ def _init_pubsub(self):
RobotState,
self.args.robot_state_topic,
self.robot_state_callback,
10)
10,
)

def _init_mqtt(self):
try:
for robot_name in ROBOT_ID_TO_AUTHKEY_MAP.keys():
self.mqtt_pubs[robot_name] = mqtt.Client(
'rbmgr_pub_' + ROBOT_ID_TO_AUTHKEY_MAP[robot_name])
'rbmgr_pub_' + ROBOT_ID_TO_AUTHKEY_MAP[robot_name]
)
self.mqtt_pubs[robot_name].connect(self.args.mqtt_server)
except ConnectionRefusedError as e:
print(f"MQTT connection to {self.args.mqtt_server} failed.")
print("Please check that the MQTT server is running!")
raise(e)
print(f'MQTT connection to {self.args.mqtt_server} failed.')
print('Please check that the MQTT server is running!')
raise (e)

def _init_gps_conversion_tools(self, frame: str):
assert frame in SUPPORTED_GPS_FRAMES

if frame == 'svy21':
self._wgs_transformer = Transformer.from_crs(
'EPSG:3414', 'EPSG:4326')
'EPSG:3414', 'EPSG:4326'
)
return

raise Exception("This should not happen")
raise Exception('This should not happen')

def _robot_state_to_gps_json(self, robot_state: RobotState):
resp = copy.deepcopy(ROBOTMANAGER_GPS_DEFINITION)

svy21_xy = self._remove_offsets(
robot_state.location.x,
robot_state.location.y
robot_state.location.x, robot_state.location.y
)
wgs84_xy = self._wgs_transformer.transform(
svy21_xy[1], svy21_xy[0]) # inputs are y,x
resp["timestamp"] = robot_state.location.t.sec
resp["robot_id"] = robot_state.name
resp["globalPose"]["lat"] = wgs84_xy[0]
resp["globalPose"]["lng"] = wgs84_xy[1]
resp["globalPose"]["alt"] = 0
resp["globalPose"]["heading"] = robot_state.location.yaw

resp["mapPose"]["x"] = robot_state.location.x
resp["mapPose"]["y"] = robot_state.location.y
resp["mapPose"]["z"] = 0.0
resp["mapPose"]["heading"] = robot_state.location.yaw

resp["batteryPct"] = robot_state.battery_percent
svy21_xy[1], svy21_xy[0]
) # inputs are y,x
resp['timestamp'] = robot_state.location.t.sec
resp['robot_id'] = robot_state.name
resp['globalPose']['lat'] = wgs84_xy[0]
resp['globalPose']['lng'] = wgs84_xy[1]
resp['globalPose']['alt'] = 0
resp['globalPose']['heading'] = robot_state.location.yaw

resp['mapPose']['x'] = robot_state.location.x
resp['mapPose']['y'] = robot_state.location.y
resp['mapPose']['z'] = 0.0
resp['mapPose']['heading'] = robot_state.location.yaw

resp['batteryPct'] = robot_state.battery_percent

return json.dumps(resp)

def _remove_offsets(self, x: float, y: float):
return (x + self.args.offset_x,
y + self.args.offset_y)
return (x + self.args.offset_x, y + self.args.offset_y)

def _apply_offsets(self, x: float, y: float):
return (x - self.args.offset_x,
y - self.args.offset_y)
return (x - self.args.offset_x, y - self.args.offset_y)


def main(argv=sys.argv):
Expand All @@ -198,13 +213,12 @@ def main(argv=sys.argv):
except KeyboardInterrupt:
pass
except OSError:
node.get_logger().error(
"Check if target port is already in use.")
node.get_logger().error('Check if target port is already in use.')
finally:
node.get_logger().info('shutting down...')
node.destroy_node()
rclpy.shutdown()


if __name__ == "__main__":
if __name__ == '__main__':
main(sys.argv)
Loading

0 comments on commit 54b48ad

Please sign in to comment.