HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
mir_robot/mir_driver/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,137 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package mir_driver
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.1.7 (2023-01-20)
------------------
* Don't set cmake_policy CMP0048
* Fix pydocstyle errors
* Add license headers
* Fix flake8 warnings
* Contributors: Martin Günther
1.1.6 (2022-06-02)
------------------
* Add arg mir_type to launch files and urdfs
* Rename mir_100 -> mir
This is in preparation of mir_250 support.
* Contributors: Martin Günther
1.1.5 (2022-02-11)
------------------
1.1.4 (2021-12-10)
------------------
1.1.3 (2021-06-11)
------------------
* Merge branch 'melodic-2.8' into noetic
* Subscribe to move_base_simple/goal in relative namespace
* Use absolute topics for /tf, /tf_static, /map etc.
* Rename tf frame and topic 'odom_comb' -> 'odom'
This is how they are called on the real MiR since MiR software 2.0.
* Fix handling of tf_static topic
This does two things:
1. Make the tf_static topic latched.
2. Cache all transforms, publish as one message.
* Increase queue_size for publishers + subscribers to 10
One case where this was a problem was the tf_static topic: Since
multiple messages are being published at once, the subscriber often
missed one. The tf_static topic will be fixed anyway in the next commit,
but let's increase the queue_size anyway to avoid such bugs in the
future.
* Update topic list to 2.8.3.1
* Reformat python code using black
* Remove outdated topics
These topics don't exist on MiR software 2.8.3 any more (most of them
have been removed a long time ago).
Fixes `#37 <https://github.com/DFKI-NI/mir_robot/issues/37>`_.
* Remove MirStatus
This message was removed in MiR software 2.0 (Renamed to RobotStatus).
* Use same MirMoveBase params as real MiR (2.8.3)
This shouldn't make a difference (it used to work before). Just removing
one more potential source of error.
* Fix: Converts move_base_simple/goal into a move_base action. (`#62 <https://github.com/DFKI-NI/mir_robot/issues/62>`_)
At least MIR software version 2.8 does not react properly to move_base_simple/goal messages. This implements a workaround.
Closes `#60 <https://github.com/DFKI-NI/mir_robot/issues/60>`_.
* Fix: Adds subscription to "tf_static". (`#58 <https://github.com/DFKI-NI/mir_robot/issues/58>`_)
Some transformations are published on this topic and are needed to
obtain a full tf tree. E.g. "base_footprint" to "base_link"
* Minor: Removes /particlecloud from the list of published topics. (`#57 <https://github.com/DFKI-NI/mir_robot/issues/57>`_)
* Fix: Add missing dict_filter keyword argument for cmd_vel msgs (`#56 <https://github.com/DFKI-NI/mir_robot/issues/56>`_)
* Remove relative_move_action (MiR => 2.4.0)
This action was merged into the generic MirMoveBaseAction in MiR
software 2.4.0.
* Adjust to changed MirMoveBase action (MiR >= 2.4.0)
See `#45 <https://github.com/DFKI-NI/mir_robot/issues/45>`_.
* Adjust cmd_vel topic to TwistStamped (MiR >= 2.7)
See `#45 <https://github.com/DFKI-NI/mir_robot/issues/45>`_.
* Contributors: Martin Günther, matthias-mayr
1.1.2 (2021-05-12)
------------------
1.1.1 (2021-02-11)
------------------
* Fix subscribing twice to same topic (TF etc)
There was a flaw in the subscriber logic that caused the mir_bridge to
subscribe multiple times to the same topic from the MiR, especially for
latched topics. This can be seen by repeated lines in the output:
starting to stream messages on topic 'tf'
starting to stream messages on topic 'tf'
starting to stream messages on topic 'tf'
Probably related to `#64 <https://github.com/DFKI-NI/mir_robot/issues/64>`_.
* Contributors: Martin Günther
1.1.0 (2020-06-30)
------------------
* Initial release into noetic
* Adapt to changes in websocket-client >= 0.49
Ubuntu 16.04 has python-websocket 0.18
Ubuntu 20.04 has python3-websocket 0.53
* Update scripts to Python3 (Noetic)
* Contributors: Martin Günther
1.0.6 (2020-06-30)
------------------
* Set cmake_policy CMP0048 to fix warning
* Contributors: Martin Günther
1.0.5 (2020-05-01)
------------------
* Add optional prefix parameter to fake_mir_joint_publisher (`#47 <https://github.com/DFKI-NI/mir_robot/issues/47>`_)
* tf_remove_child_frames: Don't publish empty TFs
* Add sdc21x0 package, MC/currents topic
* Contributors: Martin Günther, Nils Niemann
1.0.4 (2019-05-06)
------------------
* Remove garbage file
* Contributors: Martin Günther
1.0.3 (2019-03-04)
------------------
* Make disable_map work with MiR software 2.0
See `#5 <https://github.com/DFKI-NI/mir_robot/issues/5>`_.
* mir_driver: Optionally disable the map topic + TF frame (`#6 <https://github.com/DFKI-NI/mir_robot/issues/6>`_)
This is useful when running one's own SLAM / localization nodes.
Fixes `#5 <https://github.com/DFKI-NI/mir_robot/issues/5>`_.
* Split scan_rep117 topic into two separate topics
This fixes the problem that the back laser scanner was ignored in the
navigation costmap in Gazebo (probably because in Gazebo, both laser
scanners have the exact same timestamp).
* Contributors: Martin Günther
1.0.2 (2018-07-30)
------------------
1.0.1 (2018-07-17)
------------------
* mir_driver: Remove leading slashes in TF frames
* mir_driver: Install launch directory
* Contributors: Martin Günther
1.0.0 (2018-07-12)
------------------
* Initial release
* Contributors: Martin Günther

67
mir_robot/mir_driver/CMakeLists.txt Executable file
View File

@@ -0,0 +1,67 @@
cmake_minimum_required(VERSION 3.5.1)
project(mir_driver)
find_package(catkin REQUIRED COMPONENTS
actionlib_msgs
diagnostic_msgs
dynamic_reconfigure
geometry_msgs
mir_actions
mir_msgs
move_base_msgs
nav_msgs
rosgraph_msgs
roslaunch
rospy
rospy_message_converter
sensor_msgs
std_msgs
tf
visualization_msgs
)
catkin_python_setup()
###################################
## catkin specific configuration ##
###################################
catkin_package(
CATKIN_DEPENDS
actionlib_msgs
diagnostic_msgs
dynamic_reconfigure
geometry_msgs
mir_actions
mir_msgs
move_base_msgs
nav_msgs
rosgraph_msgs
rospy_message_converter
sensor_msgs
std_msgs
tf
visualization_msgs
)
#############
## Install ##
#############
catkin_install_python(PROGRAMS
nodes/fake_mir_joint_publisher.py
nodes/mir_bridge.py
nodes/rep117_filter.py
nodes/tf_remove_child_frames.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
# Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY
launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
roslaunch_add_file_check(launch)

85
mir_robot/mir_driver/launch/mir.launch Executable file
View File

@@ -0,0 +1,85 @@
<launch>
<arg name="mir_type" default="mir_250" doc="The MiR variant. Can be 'mir_100' or 'mir_250' for now." />
<arg name="tf_prefix" default="" doc="TF prefix to use for all of MiR's TF frames"/>
<arg name="mir_hostname" default="192.168.12.20" />
<arg name="disable_map" default="false" doc="Disable the map topic and map -> odom TF transform from the MiR" />
<param name="tf_prefix" type="string" value="$(arg tf_prefix)"/>
<!-- URDF -->
<include file="$(find mir_description)/launch/upload_mir_urdf.launch">
<arg name="mir_type" value="$(arg mir_type)" />
</include>
<!-- Robot state publisher -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<remap from="/tf" to="tf_rss" />
<remap from="/tf_static" to="tf_static_rss" />
</node>
<!-- remove those TFs that are also published by the MiR to avoid conflicts -->
<node name="tf_remove_state_publisher_frames" pkg="mir_driver" type="tf_remove_child_frames.py" output="screen">
<remap from="tf_in" to="tf_rss" />
<remap from="tf_out" to="/tf" />
<remap from="tf_static_in" to="tf_static_rss" />
<remap from="tf_static_out" to="/tf_static" />
<rosparam param="remove_frames">
- base_link
- front_laser_link
- back_laser_link
- camera_top_link
- camera_top_depth_optical_frame
- camera_floor_link
- camera_floor_depth_optical_frame
- imu_link
</rosparam>
</node>
<!-- MiR base -->
<group if="$(arg disable_map)">
<node name="mir_bridge" pkg="mir_driver" type="mir_bridge.py" output="screen">
<param name="hostname" value="$(arg mir_hostname)" />
<param name="tf_prefix" value="$(arg tf_prefix)" />
<remap from="map" to="map_mir" />
<remap from="map_metadata" to="map_metadata_mir" />
<remap from="rosout" to="/rosout" />
<remap from="rosout_agg" to="/rosout_agg" />
<remap from="tf" to="tf_mir" />
</node>
<!-- remove the map -> odom TF transform -->
<node name="tf_remove_mir_map_frame" pkg="mir_driver" type="tf_remove_child_frames.py" output="screen">
<remap from="tf_in" to="tf_mir" />
<remap from="tf_out" to="/tf" />
<rosparam param="remove_frames">
- odom
</rosparam>
</node>
</group>
<group unless="$(arg disable_map)">
<node name="mir_bridge" pkg="mir_driver" type="mir_bridge.py" output="screen">
<param name="hostname" value="$(arg mir_hostname)" />
<param name="tf_prefix" value="$(arg tf_prefix)" />
<remap from="map" to="/map" />
<remap from="map_metadata" to="/map_metadata" />
<remap from="rosout" to="/rosout" />
<remap from="rosout_agg" to="/rosout_agg" />
<remap from="tf" to="/tf" />
</node>
</group>
<node name="b_rep117_laser_filter" pkg="mir_driver" type="rep117_filter.py" output="screen">
<remap from="scan" to="b_scan" />
<remap from="scan_filtered" to="b_scan_rep117" />
</node>
<node name="f_rep117_laser_filter" pkg="mir_driver" type="rep117_filter.py" output="screen">
<remap from="scan" to="f_scan" />
<remap from="scan_filtered" to="f_scan_rep117" />
</node>
<node name="fake_mir_joint_publisher" pkg="mir_driver" type="fake_mir_joint_publisher.py" output="screen" />
</launch>

67
mir_robot/mir_driver/nodes/fake_mir_joint_publisher.py Executable file
View File

@@ -0,0 +1,67 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Martin Günther
import rospy
from sensor_msgs.msg import JointState
def fake_mir_joint_publisher():
rospy.init_node('fake_mir_joint_publisher')
prefix = rospy.get_param('~prefix', '')
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
r = rospy.Rate(50.0)
while not rospy.is_shutdown():
js = JointState()
js.header.stamp = rospy.Time.now()
js.name = [
prefix + 'left_wheel_joint',
prefix + 'right_wheel_joint',
prefix + 'fl_caster_rotation_joint',
prefix + 'fl_caster_wheel_joint',
prefix + 'fr_caster_rotation_joint',
prefix + 'fr_caster_wheel_joint',
prefix + 'bl_caster_rotation_joint',
prefix + 'bl_caster_wheel_joint',
prefix + 'br_caster_rotation_joint',
prefix + 'br_caster_wheel_joint',
]
js.position = [0.0 for _ in js.name]
js.velocity = [0.0 for _ in js.name]
js.effort = [0.0 for _ in js.name]
pub.publish(js)
r.sleep()
if __name__ == '__main__':
try:
fake_mir_joint_publisher()
except rospy.ROSInterruptException:
pass

529
mir_robot/mir_driver/nodes/mir_bridge.py Executable file
View File

@@ -0,0 +1,529 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Martin Günther
import rospy
import copy
import sys
from collections.abc import Iterable
from mir_driver import rosbridge
from rospy_message_converter import message_converter
from actionlib import SimpleActionClient
import actionlib_msgs.msg
import diagnostic_msgs.msg
import dynamic_reconfigure.msg
import geometry_msgs.msg
import mir_actions.msg
import mir_msgs.msg
import move_base_msgs.msg
import nav_msgs.msg
import rosgraph_msgs.msg
import sdc21x0.msg
import sensor_msgs.msg
import std_msgs.msg
import tf2_msgs.msg
import visualization_msgs.msg
from collections import OrderedDict
tf_prefix = ''
static_transforms = OrderedDict()
class TopicConfig(object):
def __init__(self, topic, topic_type, latch=False, dict_filter=None):
self.topic = topic
self.topic_type = topic_type
self.latch = latch
self.dict_filter = dict_filter
# remap mir_actions/MirMoveBaseAction => move_base_msgs/MoveBaseAction
def _move_base_goal_dict_filter(msg_dict):
filtered_msg_dict = copy.deepcopy(msg_dict)
filtered_msg_dict['goal']['move_task'] = mir_actions.msg.MirMoveBaseGoal.GLOBAL_MOVE
filtered_msg_dict['goal']['goal_dist_threshold'] = 0.25
filtered_msg_dict['goal']['clear_costmaps'] = True
return filtered_msg_dict
def _move_base_feedback_dict_filter(msg_dict):
# filter out slots from the dict that are not in our message definition
# e.g., MiRMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't
filtered_msg_dict = copy.deepcopy(msg_dict)
filtered_msg_dict['feedback'] = {
key: msg_dict['feedback'][key] for key in move_base_msgs.msg.MoveBaseFeedback.__slots__
}
return filtered_msg_dict
def _move_base_result_dict_filter(msg_dict):
filtered_msg_dict = copy.deepcopy(msg_dict)
filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in move_base_msgs.msg.MoveBaseResult.__slots__}
return filtered_msg_dict
def _cmd_vel_dict_filter(msg_dict):
"""
Convert Twist to TwistStamped.
Convert a geometry_msgs/Twist message dict (as sent from the ROS side) to
a geometry_msgs/TwistStamped message dict (as expected by the MiR on
software version >=2.7).
"""
header = std_msgs.msg.Header(frame_id='', stamp=rospy.Time.now())
filtered_msg_dict = {
'header': message_converter.convert_ros_message_to_dictionary(header),
'twist': copy.deepcopy(msg_dict),
}
return filtered_msg_dict
def _tf_dict_filter(msg_dict):
filtered_msg_dict = copy.deepcopy(msg_dict)
for transform in filtered_msg_dict['transforms']:
transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/')
return filtered_msg_dict
def _tf_static_dict_filter(msg_dict):
"""
Cache tf_static messages (simulate latching).
The tf_static topic needs special handling. Publishers on tf_static are *latched*, which means that the ROS master
caches the last message that was sent by each publisher on that topic, and will forward it to new subscribers.
However, since the mir_driver node appears to the ROS master as a single node with a single publisher on tf_static,
and there are multiple actual publishers hiding behind it on the MiR side, only one of those messages will be
cached. Therefore, we need to implement the caching ourselves and make sure that we always publish the full set of
transforms as a single message.
"""
global static_transforms
# prepend tf_prefix
filtered_msg_dict = _tf_dict_filter(msg_dict)
# The following code was copied + modified from https://github.com/tradr-project/static_transform_mux .
# Process the incoming transforms, merge them with our cache.
for transform in filtered_msg_dict['transforms']:
key = transform['child_frame_id']
rospy.loginfo(
"[%s] tf_static: updated transform %s->%s.",
rospy.get_name(),
transform['header']['frame_id'],
transform['child_frame_id'],
)
static_transforms[key] = transform
# Return the cached messages.
filtered_msg_dict['transforms'] = static_transforms.values()
rospy.loginfo(
"[%s] tf_static: sent %i transforms: %s",
rospy.get_name(),
len(static_transforms),
str(static_transforms.keys()),
)
return filtered_msg_dict
def _prepend_tf_prefix_dict_filter(msg_dict):
# filtered_msg_dict = copy.deepcopy(msg_dict)
if not isinstance(msg_dict, dict): # can happen during recursion
return
for (key, value) in msg_dict.items():
if key == 'header':
try:
# prepend frame_id
frame_id = value['frame_id'].strip('/')
if frame_id != 'map':
# prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty)
value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/')
else:
value['frame_id'] = frame_id
except TypeError:
pass # value is not a dict
except KeyError:
pass # value doesn't have key 'frame_id'
elif isinstance(value, dict):
_prepend_tf_prefix_dict_filter(value)
elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list)
for item in value:
_prepend_tf_prefix_dict_filter(item)
return msg_dict
def _remove_tf_prefix_dict_filter(msg_dict):
# filtered_msg_dict = copy.deepcopy(msg_dict)
if not isinstance(msg_dict, dict): # can happen during recursion
return
for (key, value) in msg_dict.items():
if key == 'header':
try:
# remove frame_id
s = value['frame_id'].strip('/')
if s.find(tf_prefix) == 0:
value['frame_id'] = (s[len(tf_prefix) :]).strip('/') # strip off tf_prefix, then strip leading '/'
except TypeError:
pass # value is not a dict
except KeyError:
pass # value doesn't have key 'frame_id'
elif isinstance(value, dict):
_remove_tf_prefix_dict_filter(value)
elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list)
for item in value:
_remove_tf_prefix_dict_filter(item)
return msg_dict
# topics we want to publish to ROS (and subscribe to from the MiR)
PUB_TOPICS = [
# TopicConfig('LightCtrl/bms_data', mir_msgs.msg.BMSData),
# TopicConfig('LightCtrl/charging_state', mir_msgs.msg.ChargingState),
TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range),
# TopicConfig('MC/battery_currents', mir_msgs.msg.BatteryCurrents),
# TopicConfig('MC/battery_voltage', std_msgs.msg.Float64),
TopicConfig('MC/currents', sdc21x0.msg.MotorCurrents),
# TopicConfig('MC/encoders', sdc21x0.msg.StampedEncoders),
TopicConfig('MissionController/CheckArea/visualization_marker', visualization_msgs.msg.Marker),
# TopicConfig('MissionController/goal_position_guid', std_msgs.msg.String),
# TopicConfig('MissionController/prompt_user', mir_msgs.msg.UserPrompt),
TopicConfig('SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
TopicConfig('SickPLC/parameter_updates', dynamic_reconfigure.msg.Config),
# TopicConfig('active_mapping_guid', std_msgs.msg.String),
TopicConfig('amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped),
TopicConfig('b_raw_scan', sensor_msgs.msg.LaserScan),
TopicConfig('b_scan', sensor_msgs.msg.LaserScan),
TopicConfig('camera_floor/background', sensor_msgs.msg.PointCloud2),
TopicConfig('camera_floor/depth/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
TopicConfig('camera_floor/depth/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('camera_floor/depth/points', sensor_msgs.msg.PointCloud2),
TopicConfig('camera_floor/filter/visualization_marker', visualization_msgs.msg.Marker),
TopicConfig('camera_floor/floor', sensor_msgs.msg.PointCloud2),
TopicConfig('camera_floor/obstacles', sensor_msgs.msg.PointCloud2),
TopicConfig('check_area/polygon', geometry_msgs.msg.PolygonStamped),
# TopicConfig('check_pose_area/polygon', geometry_msgs.msg.PolygonStamped),
# TopicConfig('data_events/area_events', mir_data_msgs.msg.AreaEventEvent),
# TopicConfig('data_events/maps', mir_data_msgs.msg.MapEvent),
# TopicConfig('data_events/positions', mir_data_msgs.msg.PositionEvent),
# TopicConfig('data_events/registers', mir_data_msgs.msg.PLCRegisterEvent),
# TopicConfig('data_events/sounds', mir_data_msgs.msg.SoundEvent),
TopicConfig('diagnostics', diagnostic_msgs.msg.DiagnosticArray),
TopicConfig('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray),
TopicConfig('diagnostics_toplevel_state', diagnostic_msgs.msg.DiagnosticStatus),
TopicConfig('f_raw_scan', sensor_msgs.msg.LaserScan),
TopicConfig('f_scan', sensor_msgs.msg.LaserScan),
TopicConfig('imu_data', sensor_msgs.msg.Imu),
TopicConfig('laser_back/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
TopicConfig('laser_back/driver/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('laser_front/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
TopicConfig('laser_front/driver/parameter_updates', dynamic_reconfigure.msg.Config),
# TopicConfig('localization_score', std_msgs.msg.Float64),
TopicConfig('/map', nav_msgs.msg.OccupancyGrid, latch=True),
TopicConfig('/map_metadata', nav_msgs.msg.MapMetaData),
# TopicConfig('marker_tracking_node/feedback', mir_marker_tracking.msg.MarkerTrackingActionFeedback),
# TopicConfig(
# 'marker_tracking_node/laser_line_extract/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription
# ),
# TopicConfig('marker_tracking_node/laser_line_extract/parameter_updates', dynamic_reconfigure.msg.Config),
# TopicConfig('marker_tracking_node/laser_line_extract/visualization_marker', visualization_msgs.msg.Marker),
# TopicConfig('marker_tracking_node/result', mir_marker_tracking.msg.MarkerTrackingActionResult),
# TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray),
# TopicConfig('mirEventTrigger/events', mir_msgs.msg.Events),
TopicConfig('mir_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
TopicConfig('mir_amcl/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('mir_amcl/selected_points', sensor_msgs.msg.PointCloud2),
TopicConfig('mir_log', rosgraph_msgs.msg.Log),
# TopicConfig('mir_sound/sound_event', mir_msgs.msg.SoundEvent),
TopicConfig('mir_status_msg', std_msgs.msg.String),
# TopicConfig('mirspawn/node_events', mirSpawn.msg.LaunchItem),
TopicConfig('mirwebapp/grid_map_metadata', mir_msgs.msg.LocalMapStat),
TopicConfig('mirwebapp/laser_map_metadata', mir_msgs.msg.LocalMapStat),
# TopicConfig('mirwebapp/web_path', mir_msgs.msg.WebPath),
# really mir_actions/MirMoveBaseActionFeedback:
TopicConfig(
'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter
),
# really mir_actions/MirMoveBaseActionResult:
TopicConfig('move_base/result', move_base_msgs.msg.MoveBaseActionResult, dict_filter=_move_base_result_dict_filter),
TopicConfig('move_base/status', actionlib_msgs.msg.GoalStatusArray),
# TopicConfig('move_base_node/MIRPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2),
# TopicConfig('move_base_node/MIRPlannerROS/global_plan', nav_msgs.msg.Path),
# TopicConfig('move_base_node/MIRPlannerROS/len_to_goal', std_msgs.msg.Float64),
TopicConfig('move_base_node/MIRPlannerROS/local_plan', nav_msgs.msg.Path),
# TopicConfig('move_base_node/MIRPlannerROS/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
# TopicConfig('move_base_node/MIRPlannerROS/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('move_base_node/MIRPlannerROS/updated_global_plan', mir_msgs.msg.PlanSegments),
# TopicConfig('move_base_node/MIRPlannerROS/visualization_marker', visualization_msgs.msg.MarkerArray),
TopicConfig('move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path),
# TopicConfig(
# 'move_base_node/SBPLLatticePlanner/sbpl_lattice_planner_stats', sbpl_lattice_planner.msg.SBPLLatticePlannerStats
# ),
# TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', visualization_msgs.msg.MarkerArray),
TopicConfig('move_base_node/current_goal', geometry_msgs.msg.PoseStamped),
# TopicConfig('move_base_node/global_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
# TopicConfig('move_base_node/global_costmap/obstacles', nav_msgs.msg.GridCells),
# TopicConfig('move_base_node/global_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
# TopicConfig('move_base_node/global_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
# TopicConfig('move_base_node/global_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
# TopicConfig('move_base_node/global_costmap/unknown_space', nav_msgs.msg.GridCells),
# TopicConfig('move_base_node/global_plan', nav_msgs.msg.Path),
TopicConfig('move_base_node/local_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
TopicConfig('move_base_node/local_costmap/obstacles', nav_msgs.msg.GridCells),
# TopicConfig('move_base_node/local_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
# TopicConfig('move_base_node/local_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('move_base_node/local_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
# TopicConfig('move_base_node/local_costmap/safety_zone', geometry_msgs.msg.PolygonStamped),
# TopicConfig('move_base_node/local_costmap/unknown_space', nav_msgs.msg.GridCells),
# TopicConfig('move_base_node/mir_escape_recovery/visualization_marker', visualization_msgs.msg.Marker),
# TopicConfig('move_base_node/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
# TopicConfig('move_base_node/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('move_base_node/time_to_coll', std_msgs.msg.Float64),
TopicConfig('move_base_node/traffic_costmap/inflated_obstacles', nav_msgs.msg.GridCells),
TopicConfig('move_base_node/traffic_costmap/obstacles', nav_msgs.msg.GridCells),
TopicConfig('move_base_node/traffic_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
TopicConfig('move_base_node/traffic_costmap/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('move_base_node/traffic_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped),
TopicConfig('move_base_node/traffic_costmap/unknown_space', nav_msgs.msg.GridCells),
TopicConfig('move_base_node/visualization_marker', visualization_msgs.msg.Marker),
TopicConfig('move_base_simple/visualization_marker', visualization_msgs.msg.Marker),
TopicConfig('odom', nav_msgs.msg.Odometry),
TopicConfig('odom_enc', nav_msgs.msg.Odometry),
# TopicConfig('one_way_map', nav_msgs.msg.OccupancyGrid),
# TopicConfig('param_update', std_msgs.msg.String),
# TopicConfig('particlevizmarker', visualization_msgs.msg.MarkerArray),
# TopicConfig('resource_tracker/needed_resources', mir_msgs.msg.ResourcesState),
TopicConfig('robot_mode', mir_msgs.msg.RobotMode),
TopicConfig('robot_pose', geometry_msgs.msg.Pose),
TopicConfig('robot_state', mir_msgs.msg.RobotState),
# TopicConfig('robot_status', mir_msgs.msg.RobotStatus),
TopicConfig('/rosout', rosgraph_msgs.msg.Log),
TopicConfig('/rosout_agg', rosgraph_msgs.msg.Log),
TopicConfig('scan', sensor_msgs.msg.LaserScan),
# TopicConfig('scan_filter/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
# TopicConfig('scan_filter/parameter_updates', dynamic_reconfigure.msg.Config),
TopicConfig('scan_filter/visualization_marker', visualization_msgs.msg.Marker),
# TopicConfig('session_importer_node/info', mirSessionImporter.msg.SessionImportInfo),
# TopicConfig('set_mc_PID', std_msgs.msg.Float64MultiArray),
TopicConfig('/tf', tf2_msgs.msg.TFMessage, dict_filter=_tf_dict_filter),
TopicConfig('/tf_static', tf2_msgs.msg.TFMessage, dict_filter=_tf_static_dict_filter, latch=True),
# TopicConfig('traffic_map', nav_msgs.msg.OccupancyGrid),
# TopicConfig('wifi_diagnostics', diagnostic_msgs.msg.DiagnosticArray),
# TopicConfig('wifi_diagnostics/cur_ap', mir_wifi_msgs.msg.APInfo),
# TopicConfig('wifi_diagnostics/roam_events', mir_wifi_msgs.msg.WifiRoamEvent),
# TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', mir_wifi_msgs.msg.WifiInterfaceStats),
# TopicConfig('wifi_diagnostics/wifi_ap_rssi', mir_wifi_msgs.msg.APRssiStats),
# TopicConfig('wifi_diagnostics/wifi_ap_time_stats', mir_wifi_msgs.msg.APTimeStats),
# TopicConfig('wifi_watchdog/ping', mir_wifi_msgs.msg.APPingStats),
]
# topics we want to subscribe to from ROS (and publish to the MiR)
SUB_TOPICS = [
# really geometry_msgs.msg.TwistStamped:
TopicConfig('cmd_vel', geometry_msgs.msg.Twist, dict_filter=_cmd_vel_dict_filter),
TopicConfig('initialpose', geometry_msgs.msg.PoseWithCovarianceStamped),
TopicConfig('light_cmd', std_msgs.msg.String),
TopicConfig('mir_cmd', std_msgs.msg.String),
TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID),
# really mir_actions/MirMoveBaseActionGoal:
TopicConfig('move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter),
]
class PublisherWrapper(rospy.SubscribeListener):
def __init__(self, topic_config, robot):
self.topic_config = topic_config
self.robot = robot
self.connected = False
# Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
self.pub = rospy.Publisher(
name=topic_config.topic,
data_class=topic_config.topic_type,
subscriber_listener=self,
latch=topic_config.latch,
queue_size=10,
)
rospy.loginfo(
"[%s] publishing topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
)
# latched topics must be subscribed immediately
if topic_config.latch:
self.peer_subscribe("", None, None)
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
if not self.connected:
self.connected = True
rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
self.robot.subscribe(topic=absolute_topic, callback=self.callback)
def peer_unsubscribe(self, topic_name, num_peers):
pass
# doesn't work: once ubsubscribed, robot doesn't let us subscribe again
# if self.connected and self.pub.get_num_connections() == 0 and not self.topic_config.latch:
# self.connected = False
# rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic)
# absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
# self.robot.unsubscribe(topic=absolute_topic)
def callback(self, msg_dict):
msg_dict = _prepend_tf_prefix_dict_filter(msg_dict)
if self.topic_config.dict_filter is not None:
msg_dict = self.topic_config.dict_filter(msg_dict)
msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict)
self.pub.publish(msg)
class SubscriberWrapper(object):
def __init__(self, topic_config, robot):
self.topic_config = topic_config
self.robot = robot
# Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace.
self.sub = rospy.Subscriber(
name=topic_config.topic, data_class=topic_config.topic_type, callback=self.callback, queue_size=10
)
rospy.loginfo(
"[%s] subscribing to topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type
)
def callback(self, msg):
msg_dict = message_converter.convert_ros_message_to_dictionary(msg)
msg_dict = _remove_tf_prefix_dict_filter(msg_dict)
if self.topic_config.dict_filter is not None:
msg_dict = self.topic_config.dict_filter(msg_dict)
absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
self.robot.publish(absolute_topic, msg_dict)
class MiRBridge(object):
def __init__(self):
try:
hostname = rospy.get_param('~hostname')
except KeyError:
rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
sys.exit(-1)
port = rospy.get_param('~port', 9090)
global tf_prefix
tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
self.robot = rosbridge.RosbridgeSetup(hostname, port)
r = rospy.Rate(10)
i = 1
while not self.robot.is_connected():
if rospy.is_shutdown():
sys.exit(0)
if self.robot.is_errored():
rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
sys.exit(-1)
if i % 10 == 0:
rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
i += 1
r.sleep()
rospy.loginfo('[%s] ... connected.', rospy.get_name())
topics = self.get_topics()
published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
for pub_topic in PUB_TOPICS:
PublisherWrapper(pub_topic, self.robot)
absolute_topic = '/' + pub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
if absolute_topic not in published_topics:
rospy.logwarn("[%s] topic '%s' is not published by the MiR!", rospy.get_name(), pub_topic.topic)
for sub_topic in SUB_TOPICS:
SubscriberWrapper(sub_topic, self.robot)
absolute_topic = '/' + sub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
if absolute_topic not in subscribed_topics:
rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic)
# At least with software version 2.8 there were issues when forwarding a simple goal to the robot
# This workaround converts it into an action. Check https://github.com/DFKI-NI/mir_robot/issues/60 for details.
self._move_base_client = SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction)
rospy.Subscriber("move_base_simple/goal", geometry_msgs.msg.PoseStamped, self._move_base_simple_goal_callback)
def get_topics(self):
srv_response = self.robot.callService('/rosapi/topics', msg={})
topic_names = sorted(srv_response['topics'])
topics = []
for topic_name in topic_names:
srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name})
topic_type = srv_response['type']
srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name})
has_publishers = True if len(srv_response['publishers']) > 0 else False
srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name})
has_subscribers = True if len(srv_response['subscribers']) > 0 else False
topics.append([topic_name, topic_type, has_publishers, has_subscribers])
print('Publishers:')
for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
if has_publishers:
print((' * %s [%s]' % (topic_name, topic_type)))
print('\nSubscribers:')
for (topic_name, topic_type, has_publishers, has_subscribers) in topics:
if has_subscribers:
print((' * %s [%s]' % (topic_name, topic_type)))
return topics
def _move_base_simple_goal_callback(self, msg):
if not self._move_base_client.wait_for_server(rospy.Duration(2)):
rospy.logwarn("Could not connect to 'move_base' server after two seconds. Dropping goal.")
rospy.logwarn("Did you activate 'planner' in the MIR web interface?")
return
goal = move_base_msgs.msg.MoveBaseGoal()
goal.target_pose.header = copy.deepcopy(msg.header)
goal.target_pose.pose = copy.deepcopy(msg.pose)
self._move_base_client.send_goal(goal)
def main():
rospy.init_node('mir_bridge')
MiRBridge()
rospy.spin()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass

76
mir_robot/mir_driver/nodes/rep117_filter.py Executable file
View File

@@ -0,0 +1,76 @@
#!/usr/bin/env python3
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Martin Günther
import rospy
from sensor_msgs.msg import LaserScan
pub = None
def callback(msg):
"""
Convert laser scans to REP 117 standard.
See http://www.ros.org/reps/rep-0117.html
"""
ranges_out = []
for dist in msg.ranges:
if dist < msg.range_min:
# assume "reading too close to measure",
# although it could also be "reading invalid" (nan)
ranges_out.append(float("-inf"))
elif dist > msg.range_max:
# assume "reading of no return (outside sensor range)",
# although it could also be "reading invalid" (nan)
ranges_out.append(float("inf"))
else:
ranges_out.append(dist)
msg.ranges = ranges_out
pub.publish(msg)
def main():
global pub
rospy.init_node('rep117_filter')
pub = rospy.Publisher('scan_filtered', LaserScan, queue_size=10)
rospy.Subscriber('scan', LaserScan, callback)
rospy.spin()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass

51
mir_robot/mir_driver/nodes/tf_remove_child_frames.py Executable file
View File

@@ -0,0 +1,51 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# Copyright 2016 The Cartographer Authors
# Copyright 2018 DFKI GmbH
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import rospy
from tf.msg import tfMessage
def main():
rospy.init_node('tf_remove_child_frames')
remove_frames = rospy.get_param('~remove_frames', [])
# filter tf_in topic
tf_pub = rospy.Publisher('tf_out', tfMessage, queue_size=1)
def tf_cb(msg):
msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames]
if len(msg.transforms) > 0:
tf_pub.publish(msg)
rospy.Subscriber('tf_in', tfMessage, tf_cb)
# filter tf_static_in topic
tf_static_pub = rospy.Publisher('tf_static_out', tfMessage, queue_size=1, latch=True)
def tf_static_cb(msg):
msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames]
if len(msg.transforms) > 0:
tf_static_pub.publish(msg)
rospy.Subscriber('tf_static_in', tfMessage, tf_static_cb)
rospy.spin()
if __name__ == '__main__':
main()

40
mir_robot/mir_driver/package.xml Executable file
View File

@@ -0,0 +1,40 @@
<?xml version="1.0"?>
<package format="2">
<name>mir_driver</name>
<version>1.1.7</version>
<description>A reverse ROS bridge for the MiR robot</description>
<maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
<author email="martin.guenther@dfki.de">Martin Günther</author>
<license>BSD</license>
<license>Apache 2.0</license>
<url type="website">https://github.com/DFKI-NI/mir_robot</url>
<url type="repository">https://github.com/DFKI-NI/mir_robot</url>
<url type="bugtracker">https://github.com/DFKI-NI/mir_robot/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslaunch</build_depend>
<depend>actionlib_msgs</depend>
<depend>diagnostic_msgs</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>mir_actions</depend>
<depend>mir_msgs</depend>
<depend>move_base_msgs</depend>
<depend>nav_msgs</depend>
<depend>python3-websocket</depend>
<depend>rosgraph_msgs</depend>
<depend>rospy</depend>
<depend>rospy_message_converter</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>visualization_msgs</depend>
<exec_depend>mir_description</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
</package>

9
mir_robot/mir_driver/setup.py Executable file
View File

@@ -0,0 +1,9 @@
# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(packages=['mir_driver'], package_dir={'': 'src'})
setup(**setup_args)

0
mir_robot/mir_driver/src/mir_driver/__init__.py Executable file
View File

214
mir_robot/mir_driver/src/mir_driver/rosbridge.py Executable file
View File

@@ -0,0 +1,214 @@
# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import websocket
import threading
import json
import traceback
import time
import string
import random
class RosbridgeSetup:
def __init__(self, host, port):
self.callbacks = {}
self.service_callbacks = {}
self.resp = None
self.connection = RosbridgeWSConnection(host, port)
self.connection.registerCallback(self.onMessageReceived)
def publish(self, topic, obj):
pub = {"op": "publish", "topic": topic, "msg": obj}
self.send(pub)
def subscribe(self, topic, callback, throttle_rate=-1):
if self.addCallback(topic, callback):
sub = {"op": "subscribe", "topic": topic}
if throttle_rate > 0:
sub['throttle_rate'] = throttle_rate
self.send(sub)
def unhook(self, callback):
keys_for_deletion = []
for key, values in self.callbacks.items():
for value in values:
if callback == value:
print("Found!")
values.remove(value)
if len(values) == 0:
keys_for_deletion.append(key)
for key in keys_for_deletion:
self.unsubscribe(key)
self.callbacks.pop(key)
def unsubscribe(self, topic):
unsub = {"op": "unsubscribe", "topic": topic}
self.send(unsub)
def callService(self, serviceName, callback=None, msg=None):
id = self.generate_id()
call = {"op": "call_service", "id": id, "service": serviceName}
if msg is not None:
call['args'] = msg
if callback is None:
self.resp = None
def internalCB(msg):
self.resp = msg
return None
self.addServiceCallback(id, internalCB)
self.send(call)
while self.resp is None:
time.sleep(0.01)
return self.resp
self.addServiceCallback(id, callback)
self.send(call)
return None
def send(self, obj):
try:
self.connection.sendString(json.dumps(obj))
except Exception:
traceback.print_exc()
raise
def generate_id(self, chars=16):
return ''.join(random.SystemRandom().choice(string.ascii_letters + string.digits) for _ in range(chars))
def addServiceCallback(self, id, callback):
self.service_callbacks[id] = callback
def addCallback(self, topic, callback):
if topic in self.callbacks:
self.callbacks[topic].append(callback)
return False
self.callbacks[topic] = [callback]
return True
def is_connected(self):
return self.connection.connected
def is_errored(self):
return self.connection.errored
def onMessageReceived(self, message):
try:
# Load the string into a JSON object
obj = json.loads(message)
# print "Received: ", obj
if 'op' in obj:
option = obj['op']
if option == "publish": # A message from a topic we have subscribed to..
topic = obj["topic"]
msg = obj["msg"]
if topic in self.callbacks:
for callback in self.callbacks[topic]:
try:
callback(msg)
except Exception:
print("exception on callback", callback, "from", topic)
traceback.print_exc()
raise
elif option == "service_response":
if "id" in obj:
id = obj["id"]
values = obj["values"]
if id in self.service_callbacks:
try:
# print 'id:', id, 'func:', self.service_callbacks[id]
self.service_callbacks[id](values)
except Exception:
print("exception on callback ID:", id)
traceback.print_exc()
raise
else:
print("Missing ID!")
else:
print("Recieved unknown option - it was: ", option)
else:
print("No OP key!")
except Exception:
print("exception in onMessageReceived")
print("message", message)
traceback.print_exc()
raise
class RosbridgeWSConnection:
def __init__(self, host, port):
self.ws = websocket.WebSocketApp(
("ws://%s:%d/" % (host, port)), on_message=self.on_message, on_error=self.on_error, on_close=self.on_close
)
self.ws.on_open = self.on_open
self.run_thread = threading.Thread(target=self.run)
self.run_thread.start()
self.connected = False
self.errored = False
self.callbacks = []
def on_open(self):
print("### ROS bridge connected ###")
self.connected = True
def sendString(self, message):
if not self.connected:
print("Error: not connected, could not send message")
# TODO: throw exception
else:
self.ws.send(message)
def on_error(self, error):
self.errored = True
print("Error: %s" % error)
def on_close(self):
self.connected = False
print("### ROS bridge closed ###")
def run(self, *args):
self.ws.run_forever()
def on_message(self, message):
# Call the handlers
for callback in self.callbacks:
callback(message)
def registerCallback(self, callback):
self.callbacks.append(callback)