git commit -m "first commit"
This commit is contained in:
166
simulators/third_party/dynamic_rviz_config/scripts/config.py
vendored
Executable file
166
simulators/third_party/dynamic_rviz_config/scripts/config.py
vendored
Executable file
@@ -0,0 +1,166 @@
|
||||
#!/usr/bin/python
|
||||
#-*- coding: utf-8 -*-
|
||||
|
||||
'''
|
||||
******************************************************************************************
|
||||
* Copyright (C) 2023 Yang Haodong, All Rights Reserved *
|
||||
* *
|
||||
* @brief rviz basic pannel configure class. *
|
||||
* @author Haodong Yang *
|
||||
* @version 1.0.1 *
|
||||
* @date 2022.07.06 *
|
||||
* @license GNU General Public License (GPL) *
|
||||
******************************************************************************************
|
||||
'''
|
||||
import yaml
|
||||
|
||||
class RVizConfig:
|
||||
def __init__(self, rviz_name, path, base_file=None):
|
||||
# Rviz visualization manager
|
||||
self.VIZ_MAN = 'Visualization Manager'
|
||||
# Rviz standard tools
|
||||
self.STD_TOOLS = ['rviz/Interact', 'rviz/MoveCamera', 'rviz/Select', 'rviz/FocusCamera', 'rviz/Measure', 'rviz/PublishPoint']
|
||||
# Rviz standard panels
|
||||
self.STD_PANELS = ['rviz/Displays', 'rviz/Tool Properties']
|
||||
# Rviz file name
|
||||
self.file_name = rviz_name
|
||||
# Rviz cache
|
||||
self.cache_path = path
|
||||
# Rviz pre-configure
|
||||
self.data = yaml.load(base_file) if base_file else {}
|
||||
|
||||
def __repr__(self):
|
||||
return yaml.dump(self.data, default_flow_style=False)
|
||||
|
||||
# @breif: set Rviz configure path
|
||||
#
|
||||
# @param[in]: path Rviz configure path
|
||||
# @return: None
|
||||
def setFileCache(self, path):
|
||||
self.cache_path = path
|
||||
|
||||
# @breif: extract all configure data of `rviz Visualization Manager`
|
||||
#
|
||||
# @param[in]: None
|
||||
# @return: all configure data of `rviz Visualization Manager`
|
||||
def getVisualization(self):
|
||||
if self.VIZ_MAN not in self.data:
|
||||
self.data[self.VIZ_MAN] = {}
|
||||
return self.data[self.VIZ_MAN]
|
||||
|
||||
# @breif: set display format
|
||||
# @param[in]: displays display format
|
||||
# @return: True if successful else False
|
||||
def setDisplays(self, displays):
|
||||
if 'Displays' not in self.getVisualization():
|
||||
self.data[self.VIZ_MAN]['Displays'] = displays
|
||||
print("Displays have been set successfully!")
|
||||
return True
|
||||
print("Displays setting failed! please call displays delete first!")
|
||||
return False
|
||||
|
||||
# @breif: clear display
|
||||
# @param[in]: None
|
||||
# @return: True if successful else False
|
||||
def delDisplays(self):
|
||||
if 'Displays' in self.getVisualization():
|
||||
del self.data[self.VIZ_MAN]['Displays']
|
||||
print("Displays have been deleted successfully!")
|
||||
return True
|
||||
print("Displays delete failed! please call displays setting first!")
|
||||
return False
|
||||
|
||||
# @breif: set Rviz standard tools
|
||||
# @param[in]: None
|
||||
# @return: True if successful else False
|
||||
def setStdTools(self):
|
||||
v = self.getVisualization()
|
||||
if 'Tools' not in v:
|
||||
v['Tools'] = []
|
||||
for tool in self.STD_TOOLS:
|
||||
v['Tools'].append({'Class': tool})
|
||||
print("standard tools have been set successfully!")
|
||||
return True
|
||||
print("standard tools setting failed! please call standard tools delete first!")
|
||||
return False
|
||||
|
||||
# @breif: clear Rviz standard tools
|
||||
# @param[in]: None
|
||||
# @return: True if successful else False
|
||||
def delStdTools(self):
|
||||
if 'Tools' in self.getVisualization():
|
||||
del self.data[self.VIZ_MAN]['Tools']
|
||||
print("standard tools have been deleted successfully!")
|
||||
return True
|
||||
print("standard tools delete failed! please call standard tools setting first!")
|
||||
return False
|
||||
|
||||
# @breif: set Rviz standard panels
|
||||
# @param[in]: None
|
||||
# @return: True if successful else False
|
||||
def setStdPanels(self):
|
||||
if 'Panels' not in self.data:
|
||||
self.data['Panels'] = []
|
||||
for tool in self.STD_PANELS:
|
||||
self.data['Panels'].append({'Class': tool, 'Name': tool.split('/')[-1]})
|
||||
print("standard panels have been set successfully!")
|
||||
return True
|
||||
print("standard panels setting failed! please call standard panels delete first!")
|
||||
return False
|
||||
|
||||
# @breif: clear Rviz standard panels
|
||||
# @param[in]: None
|
||||
# @return: True if successful else False
|
||||
def delStdPanels(self):
|
||||
if 'Panels' in self.data:
|
||||
del self.data['Panels']
|
||||
print("standard panels have been deleted successfully!")
|
||||
return True
|
||||
print("standard panels delete failed! please call standard panels setting first!")
|
||||
return False
|
||||
|
||||
# @breif: set base coordinate
|
||||
# @param[in]: frame base coordinate, default is `map`
|
||||
# @return: None
|
||||
def setFixedFrame(self, frame='map'):
|
||||
v = self.getVisualization()
|
||||
if 'Global Options' not in v:
|
||||
v['Global Options'] = {'Fixed Frame':frame}
|
||||
else:
|
||||
v['Global Options']['Fixed Frame'] = frame
|
||||
|
||||
# @breif: set the topics of Rviz standard tools
|
||||
# @param[in]: name Rviz standard tools classes
|
||||
# @param[in]: topic the topic of Rviz standard tools relatively
|
||||
# @return: None
|
||||
def setToolTopic(self, name, topic):
|
||||
for m in self.data[self.VIZ_MAN]['Tools']:
|
||||
if m.get('Class', '')==name:
|
||||
m['Topic'] = topic
|
||||
|
||||
# @breif: append navigation tools and their topics
|
||||
# @param[in]: None
|
||||
# @return: None
|
||||
def appendToolGoal(self, topic):
|
||||
v = self.getVisualization()
|
||||
if 'Tools' not in v:
|
||||
v['Tools'] = []
|
||||
v['Tools'].append({'Class': 'rviz/SetGoal', 'Topic': topic})
|
||||
|
||||
# @breif: append initial Pose and its topic
|
||||
# @param[in]: None
|
||||
# @return: None
|
||||
def appendToolInit(self, topic):
|
||||
v = self.getVisualization()
|
||||
if 'Tools' not in v:
|
||||
v['Tools'] = []
|
||||
v['Tools'].append({'Class': 'rviz/SetInitialPose', 'Topic': topic})
|
||||
|
||||
# @breif: run Rviz and apply user configure
|
||||
# @param[in]: None
|
||||
# @return: None
|
||||
def run(self):
|
||||
with open( self.cache_path + self.file_name + '.rviz', "w") as fp:
|
||||
fp.write(str(self))
|
||||
import subprocess
|
||||
subprocess.call(['rosrun','rviz','rviz', '-d', self.cache_path + self.file_name + '.rviz'])
|
||||
93
simulators/third_party/dynamic_rviz_config/scripts/displays.py
vendored
Executable file
93
simulators/third_party/dynamic_rviz_config/scripts/displays.py
vendored
Executable file
@@ -0,0 +1,93 @@
|
||||
#!/usr/bin/python
|
||||
#-*- coding: utf-8 -*-
|
||||
|
||||
'''
|
||||
******************************************************************************************
|
||||
* Copyright (C) 2023 Yang Haodong, All Rights Reserved *
|
||||
* *
|
||||
* @brief rviz display pannel configure class. *
|
||||
* @author Haodong Yang *
|
||||
* @version 1.0.1 *
|
||||
* @date 2022.07.06 *
|
||||
* @license GNU General Public License (GPL) *
|
||||
******************************************************************************************
|
||||
'''
|
||||
|
||||
class Displays(list):
|
||||
def __init__(self):
|
||||
None
|
||||
|
||||
|
||||
def addDisplay(self, name, class_name, topic=None, color=None, fields={}, enabled=True):
|
||||
d = {'Name': name, 'Class': class_name, 'Enabled': enabled}
|
||||
if topic:
|
||||
d['Topic'] = topic
|
||||
if color:
|
||||
d['Color'] = '%d; %d; %d'%color
|
||||
d.update(fields)
|
||||
self.append(d)
|
||||
|
||||
def addMap(self, topic='/map', name='Map', alpha=None, scheme=None):
|
||||
fields = {}
|
||||
if alpha:
|
||||
fields['Alpha'] = alpha
|
||||
if scheme:
|
||||
fields['Color Scheme'] = scheme
|
||||
self.addDisplay(name, 'rviz/Map', topic, fields=fields)
|
||||
|
||||
def addGrid(self):
|
||||
self.addDisplay('Grid', 'rviz/Grid')
|
||||
|
||||
def addTf(self, scale=None):
|
||||
fields = {}
|
||||
if scale:
|
||||
fields['Marker Scale'] = scale
|
||||
self.addDisplay('TF', 'rviz/TF', fields=fields, enabled=False)
|
||||
|
||||
def addGroup(self, name, displays):
|
||||
self.addDisplay(name, 'rviz/Group', fields={'Displays': displays})
|
||||
|
||||
def addModel(self, parameter='robot_description', tf_prefix=None):
|
||||
fields = {'Robot Description': parameter}
|
||||
if tf_prefix:
|
||||
fields['TF Prefix'] = tf_prefix
|
||||
self.addDisplay('RobotModel', 'rviz/RobotModel', fields=fields)
|
||||
|
||||
def addLaserscan(self, topic='/base_scan', name=None, color=(46, 255, 0), size=0.1, alpha=None):
|
||||
if name is None:
|
||||
name = topic
|
||||
fields = {'Size (m)': size, 'Style': 'Spheres', 'Color Transformer': 'FlatColor'}
|
||||
if alpha:
|
||||
fields['Alpha'] = alpha
|
||||
self.addDisplay(name, 'rviz/LaserScan', topic, color, fields)
|
||||
|
||||
def addPoseArray(self, topic='/particlecloud', color=None):
|
||||
self.addDisplay('AMCL Cloud', 'rviz/PoseArray', topic, color)
|
||||
|
||||
def addFootprint(self, topic, color=(0,170,255)):
|
||||
self.addDisplay('Robot Footprint', 'rviz/Polygon', topic, color)
|
||||
|
||||
def addPath(self, topic, name, color=None):
|
||||
self.addDisplay(name, 'rviz/Path', topic, color)
|
||||
|
||||
def addPose(self, topic, name='Current Goal', color=None, arrow_shape=None):
|
||||
fields = {}
|
||||
if arrow_shape:
|
||||
fields['Head Length'] = arrow_shape[0]
|
||||
fields['Head Radius'] = arrow_shape[1]
|
||||
fields['Shaft Length'] = arrow_shape[2]
|
||||
fields['Shaft Radius'] = arrow_shape[3]
|
||||
fields['Shape'] = 'Arrow'
|
||||
self.addDisplay(name, 'rviz/Pose', topic, color, fields)
|
||||
|
||||
def addImage(self, topic="/camera/rgb/image_raw", name="Image"):
|
||||
self.addDisplay(name, "rviz/Image", topic, enabled=False)
|
||||
|
||||
|
||||
|
||||
import yaml
|
||||
|
||||
def display_representer(dumper, data):
|
||||
return dumper.represent_sequence(u'tag:yaml.org,2002:seq', list(data))
|
||||
|
||||
yaml.add_representer(Displays, display_representer)
|
||||
73
simulators/third_party/dynamic_rviz_config/scripts/rviz_generate.py
vendored
Executable file
73
simulators/third_party/dynamic_rviz_config/scripts/rviz_generate.py
vendored
Executable file
@@ -0,0 +1,73 @@
|
||||
#!/usr/bin/python
|
||||
#-*- coding: utf-8 -*-
|
||||
|
||||
'''
|
||||
******************************************************************************************
|
||||
* Copyright (C) 2023 Yang Haodong, All Rights Reserved *
|
||||
* *
|
||||
* @brief rviz basic pannel configure class. *
|
||||
* @author Haodong Yang *
|
||||
* @version 1.0.1 *
|
||||
* @date 2022.07.06 *
|
||||
* @license GNU General Public License (GPL) *
|
||||
******************************************************************************************
|
||||
'''
|
||||
import sys, os
|
||||
dir_path = os.path.dirname(os.path.realpath(__file__))
|
||||
sys.path.insert(0, dir_path)
|
||||
|
||||
from config import RVizConfig
|
||||
from displays import Displays
|
||||
|
||||
# the number of robots
|
||||
robot_num = int(sys.argv[1])
|
||||
|
||||
r = RVizConfig("cache", os.path.split(os.path.realpath(__file__))[0] + "/../../../sim_env/rviz/")
|
||||
# set Rviz standard tools
|
||||
r.setStdTools()
|
||||
# visualize Rviz models
|
||||
d = Displays()
|
||||
# load map and grid
|
||||
d.addMap()
|
||||
d.addGrid()
|
||||
# set transform
|
||||
d.addTf()
|
||||
|
||||
# single robot
|
||||
if robot_num == 1:
|
||||
# append robots' model
|
||||
d.addModel()
|
||||
# append camera image
|
||||
d.addImage()
|
||||
# append laser
|
||||
d.addLaserscan(topic="/scan", name="LaserScan", color=(255, 0, 0), size=0.03, alpha=1)
|
||||
# append `initialpose` setting panel
|
||||
r.appendToolInit('/initialpose')
|
||||
# append `move_base_simple/goal` setting panel
|
||||
r.appendToolGoal('/move_base_simple/goal')
|
||||
else:
|
||||
# multi-robot
|
||||
for i in range(robot_num):
|
||||
meta_robot = Displays()
|
||||
# append robots' model
|
||||
meta_robot.addModel(parameter="robot" + str(i + 1) + "/robot_description", tf_prefix="robot" + str(i + 1))
|
||||
# append camera image
|
||||
meta_robot.addImage(topic="/robot" + str(i + 1) + "/camera/rgb/image_raw", name="Image" + str(i + 1))
|
||||
# append laser
|
||||
meta_robot.addLaserscan(topic="/robot" + str(i + 1) + "/scan", name="LaserScan" + str(i + 1), color=(255, 0, 0), size=0.03, alpha=1)
|
||||
# append path
|
||||
meta_robot.addPath(topic=None, name="Path", color=(32, 74, 135))
|
||||
d.addGroup("Robot" + str(i + 1), meta_robot)
|
||||
# append `initialpose` setting panel
|
||||
r.appendToolInit('/robot' + str(i + 1) + '/initialpose')
|
||||
# ppend `move_base_simple/goal` setting panel
|
||||
r.appendToolGoal('/robot' + str(i + 1) +'/move_base_simple/goal')
|
||||
|
||||
# 设置Rviz标准面板
|
||||
r.setStdPanels()
|
||||
# 设置Rviz模型显示
|
||||
r.setDisplays(d)
|
||||
# 设置Rviz基坐标系
|
||||
r.setFixedFrame()
|
||||
# 启动Rviz
|
||||
r.run()
|
||||
Reference in New Issue
Block a user