|
|
|
|
@@ -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 cititruck_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 cititruck_actions.msg
|
|
|
|
|
import cititruck_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 cititruck_actions/cititruckMoveBaseAction => 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'] = cititruck_actions.msg.cititruckMoveBaseGoal.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., cititruckMoveBaseFeedback 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 cititruck 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 cititruck_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 cititruck 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 cititruck)
|
|
|
|
|
PUB_TOPICS = [
|
|
|
|
|
# TopicConfig('LightCtrl/bms_data', cititruck_msgs.msg.BMSData),
|
|
|
|
|
# TopicConfig('LightCtrl/charging_state', cititruck_msgs.msg.ChargingState),
|
|
|
|
|
TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range),
|
|
|
|
|
# TopicConfig('MC/battery_currents', cititruck_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', cititruck_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', cititruck_data_msgs.msg.AreaEventEvent),
|
|
|
|
|
# TopicConfig('data_events/maps', cititruck_data_msgs.msg.MapEvent),
|
|
|
|
|
# TopicConfig('data_events/positions', cititruck_data_msgs.msg.PositionEvent),
|
|
|
|
|
# TopicConfig('data_events/registers', cititruck_data_msgs.msg.PLCRegisterEvent),
|
|
|
|
|
# TopicConfig('data_events/sounds', cititruck_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', cititruck_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', cititruck_marker_tracking.msg.MarkerTrackingActionResult),
|
|
|
|
|
# TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray),
|
|
|
|
|
# TopicConfig('cititruckEventTrigger/events', cititruck_msgs.msg.Events),
|
|
|
|
|
TopicConfig('cititruck_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
|
|
|
|
|
TopicConfig('cititruck_amcl/parameter_updates', dynamic_reconfigure.msg.Config),
|
|
|
|
|
TopicConfig('cititruck_amcl/selected_points', sensor_msgs.msg.PointCloud2),
|
|
|
|
|
TopicConfig('cititruck_log', rosgraph_msgs.msg.Log),
|
|
|
|
|
# TopicConfig('cititruck_sound/sound_event', cititruck_msgs.msg.SoundEvent),
|
|
|
|
|
TopicConfig('cititruck_status_msg', std_msgs.msg.String),
|
|
|
|
|
# TopicConfig('cititruckspawn/node_events', cititruckSpawn.msg.LaunchItem),
|
|
|
|
|
TopicConfig('cititruckwebapp/grid_map_metadata', cititruck_msgs.msg.LocalMapStat),
|
|
|
|
|
TopicConfig('cititruckwebapp/laser_map_metadata', cititruck_msgs.msg.LocalMapStat),
|
|
|
|
|
# TopicConfig('cititruckwebapp/web_path', cititruck_msgs.msg.WebPath),
|
|
|
|
|
# really cititruck_actions/cititruckMoveBaseActionFeedback:
|
|
|
|
|
TopicConfig(
|
|
|
|
|
'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter
|
|
|
|
|
),
|
|
|
|
|
# really cititruck_actions/cititruckMoveBaseActionResult:
|
|
|
|
|
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/cititruckPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2),
|
|
|
|
|
# TopicConfig('move_base_node/cititruckPlannerROS/global_plan', nav_msgs.msg.Path),
|
|
|
|
|
# TopicConfig('move_base_node/cititruckPlannerROS/len_to_goal', std_msgs.msg.Float64),
|
|
|
|
|
TopicConfig('move_base_node/cititruckPlannerROS/local_plan', nav_msgs.msg.Path),
|
|
|
|
|
# TopicConfig('move_base_node/cititruckPlannerROS/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription),
|
|
|
|
|
# TopicConfig('move_base_node/cititruckPlannerROS/parameter_updates', dynamic_reconfigure.msg.Config),
|
|
|
|
|
TopicConfig('move_base_node/cititruckPlannerROS/updated_global_plan', cititruck_msgs.msg.PlanSegments),
|
|
|
|
|
# TopicConfig('move_base_node/cititruckPlannerROS/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/cititruck_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', cititruck_msgs.msg.ResourcesState),
|
|
|
|
|
TopicConfig('robot_mode', cititruck_msgs.msg.RobotMode),
|
|
|
|
|
TopicConfig('robot_pose', geometry_msgs.msg.Pose),
|
|
|
|
|
TopicConfig('robot_state', cititruck_msgs.msg.RobotState),
|
|
|
|
|
# TopicConfig('robot_status', cititruck_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', cititruckSessionImporter.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', cititruck_wifi_msgs.msg.APInfo),
|
|
|
|
|
# TopicConfig('wifi_diagnostics/roam_events', cititruck_wifi_msgs.msg.WifiRoamEvent),
|
|
|
|
|
# TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', cititruck_wifi_msgs.msg.WifiInterfaceStats),
|
|
|
|
|
# TopicConfig('wifi_diagnostics/wifi_ap_rssi', cititruck_wifi_msgs.msg.APRssiStats),
|
|
|
|
|
# TopicConfig('wifi_diagnostics/wifi_ap_time_stats', cititruck_wifi_msgs.msg.APTimeStats),
|
|
|
|
|
# TopicConfig('wifi_watchdog/ping', cititruck_wifi_msgs.msg.APPingStats),
|
|
|
|
|
]
|
|
|
|
|
|
|
|
|
|
# topics we want to subscribe to from ROS (and publish to the cititruck)
|
|
|
|
|
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('cititruck_cmd', std_msgs.msg.String),
|
|
|
|
|
TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID),
|
|
|
|
|
# really cititruck_actions/cititruckMoveBaseActionGoal:
|
|
|
|
|
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 cititruck 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 cititruck 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 cititruck comm
|
|
|
|
|
self.robot.publish(absolute_topic, msg_dict)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class cititruckBridge(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 cititruck comm
|
|
|
|
|
if absolute_topic not in published_topics:
|
|
|
|
|
rospy.logwarn("[%s] topic '%s' is not published by the cititruck!", 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 cititruck comm
|
|
|
|
|
if absolute_topic not in subscribed_topics:
|
|
|
|
|
rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the cititruck!", 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/cititruck_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 cititruck 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('cititruck_bridge')
|
|
|
|
|
cititruckBridge()
|
|
|
|
|
rospy.spin()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
|
try:
|
|
|
|
|
main()
|
|
|
|
|
except rospy.ROSInterruptException:
|
|
|
|
|
pass
|
