def generate_launch_description():
# Get URDF via xacro
racecar_description_share_dir = os.path.join(
get_package_share_directory('racecar_description'))
urdf_file_name = 'racecar.urdf.xml'
xacro_file_name = 'racecar.xacro.xml'
xacro_file = os.path.join(racecar_description_share_dir, 'urdf',
urdf_file_name)
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
robot_description = {"robot_description": doc.toxml()}
racecar_controller = os.path.join(
get_package_share_directory("racecar_control"),
"config",
"racecar_control.yaml",
)
return LaunchDescription([
Node(
package="controller_manager",
executable="ros2_control_node",
namespace='racecar',
parameters=[robot_description, racecar_controller],
output={
"stdout": "screen",
"stderr": "screen"
},
)
])
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
gazebo_ros2_control_demos_path = os.path.join(
get_package_share_directory('gazebo_ros2_control_demos'))
xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf',
'test_cart_position.xacro.urdf')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
params = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params])
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'cartpole'],
output='screen')
return LaunchDescription([
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def _add_cb(self, req):
name = req.name
xacro_name = os.path.join(self._urdf_dir, name + '_macro.urdf.xacro')
macro_name = 'assy_part_' + re.split('[_-]', name)[0]
position = req.pose.pose.position
orientation = req.pose.pose.orientation
xacro_desc = ModelSpawnerServer._Model.format(
name, xacro_name, macro_name,
req.prefix, req.pose.header.frame_id,
position.x, position.y, position.z,
*tfs.euler_from_quaternion([orientation.x,
orientation.y,
orientation.z,
orientation.w]))
try:
# Expand and process Xacro into XML format.
doc = xacro.parse(xacro_desc) # Create DOM tree.
xacro.process_doc(doc) # Expand and process macros.
desc = doc.toprettyxml(indent=' ', encoding='utf8')
self._models[req.prefix + name] \
= xml.dom.minidom.parseString(desc).childNodes[0]
self._publisher.publish(mmsg.ModelDescription.ADD,
req.prefix + name, desc)
return msrv.AddResponse(True)
except Exception as e:
rospy.logerr(e)
return msrv.AddResponse(False)
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
gazebo_ros2_control_demos_path = os.path.join(
get_package_share_directory('gazebo_ros2_control_demos'))
xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf',
'test_cart_velocity.xacro.urdf')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
params = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params])
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'cartpole'],
output='screen')
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_state_broadcaster'
],
output='screen')
load_joint_trajectory_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'velocity_controller'
],
output='screen')
load_imu_sensor_broadcaster = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', 'imu_sensor_broadcaster'],
output='screen')
return LaunchDescription([
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_trajectory_controller,
on_exit=[load_imu_sensor_broadcaster],
)),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def generate_launch_description():
# robot state publisher
my_rotate_bot_path = os.path.join(
get_package_share_directory('my_rotate_bot'))
urdf_file = os.path.join(my_rotate_bot_path, 'urdf', 'model.urdf')
doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description])
# gazebo
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
# spawn robot
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'rotate_box_bot'],
output='screen')
return LaunchDescription([
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def quick_xacro(xml, cli=None, **kwargs):
if cli:
opts, _ = xacro.process_cli_args(cli, require_input=False)
kwargs.update(vars(opts))
doc = xacro.parse(xml)
xacro.process_doc(doc, **kwargs)
return doc
def quick_xacro(self, xml, cli=None, **kwargs):
args = {}
if cli:
opts, _ = xacro.cli.process_args(cli, require_input=False)
args.update(vars(opts)) # initialize with cli args
args.update(kwargs) # explicit function args have highest priority
doc = xacro.parse(xml)
xacro.process_doc(doc, **args)
return doc
def quick_xacro(self, xml, cli=None, **kwargs):
args = {}
if cli:
opts, _ = xacro.cli.process_args(cli, require_input=False)
args.update(vars(opts)) # initialize with cli args
args.update(dict(in_order = self.in_order)) # set in_order option from test class
args.update(kwargs) # explicit function args have highest priority
doc = xacro.parse(xml)
xacro.process_doc(doc, **args)
return doc
def xacro_string_to_payload(xacro_str, xacro_fname=None, **kwargs):
if xacro_fname is not None:
xacro.restore_filestack([xacro_fname])
else:
xacro.restore_filestack([])
if 'mappings' not in kwargs:
kwargs['mappings'] = {'payload_extensions': 'true'}
doc = xacro.parse(xacro_str)
xacro.process_doc(doc, **kwargs)
urdf_str = doc.toprettyxml(indent=' ')
return urdf_to_payload(urdf_str)
def _get_configured_urdf(self, config):
"""
Call the XACRO to URDF conversion using the parameters given in `config`.
:param dynamic_reconfigure.Config config: Arguments of the XACRO model.
:return: The URDF file contents.
:rtype: basestring
"""
doc = xacro.parse(None, self._xacro_file)
self.__remove_comments_from_xml_doc(doc)
xacro.process_doc(doc,
mappings=dict([(key, str(val))
for (key, val) in config.iteritems()
]),
in_order=True)
return doc.toxml()
def generate_launch_description():
# robot state publisher
my_rotate_bot_path = os.path.join(
get_package_share_directory(PACKAGE_NAME))
urdf_file = os.path.join(my_rotate_bot_path, 'urdf', 'model.urdf')
doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description])
node_joint_state_publisher = Node(package='joint_state_publisher',
executable='joint_state_publisher',
output='screen',
parameters=[robot_description])
rviz_config_dir = os.path.join(get_package_share_directory(PACKAGE_NAME),
'config', 'rviz.config.rviz')
# ,
rviz = Node(package='rviz2',
node_executable='rviz2',
node_name='rviz2',
output='screen',
arguments=['-d', rviz_config_dir])
joint_state_publisher_gui = Node(
package='joint_state_publisher_gui',
node_executable='joint_state_publisher_gui',
node_name='joint_state_gui',
output='screen')
return LaunchDescription([
rviz,
node_joint_state_publisher,
joint_state_publisher_gui,
node_robot_state_publisher,
])
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
bbot_path = os.path.join(get_package_share_directory('bbot'))
xacro_file = os.path.join(bbot_path, 'sdf', 'robot.xacro.sdf')
sdf_file = os.path.join(bbot_path, 'sdf', 'robot.sdf')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
# Open a file
fd = os.open(sdf_file, os.O_RDWR | os.O_CREAT)
os.ftruncate(fd, 0)
os.lseek(fd, 0, os.SEEK_SET)
b = str.encode(doc.toxml())
os.write(fd, b)
# Close opened file
os.close(fd)
spawn_entity = Node(package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-entity', 'bbot', '-file', sdf_file, '-x',
'0.061475', '-y', '-0.058621', '-z', '0.058962',
'-R', '-0.000043', '-P', '0.563955', '-Y',
'0.425422'
],
output='screen')
return LaunchDescription([
gazebo,
spawn_entity,
])
def generate_launch_description():
# robot state publisher
my_rotate_bot_path = os.path.join(
get_package_share_directory('my_rotate_bot'))
urdf_file = os.path.join(my_rotate_bot_path, 'urdf', 'model.urdf')
doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
# ros_control
controllers_file = os.path.join(my_rotate_bot_path, 'controllers',
'ros2_control_controllers.yaml')
controller_manager = Node(package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, controllers_file],
output='screen')
return LaunchDescription([
controller_manager,
])
def __init__(self, description_file=None, load=True):
if description_file is None and len(sys.argv) > 1:
description_file = sys.argv[1]
self._save_files = rospy.get_param('~save_files', False)
self._path_to_save_files = rospy.get_param('~path_to_save_files',
"/tmp/")
self._file_name = rospy.get_param('~file_name', "generated_robot")
# ARM
self.rospack = rospkg.RosPack()
self.package_path = self.rospack.get_path('sr_multi_moveit_config')
self.robot = Robot()
with open(description_file, "r") as stream:
yamldoc = yaml.load(stream)
self.robot.set_parameters(yamldoc)
new_srdf_file_name = "generated_robot.srdf"
self.start_new_srdf(new_srdf_file_name)
for manipulator in self.robot.manipulators:
if manipulator.has_arm:
# Read arm srdf
arm_srdf_path = manipulator.arm.moveit_path + "/" + manipulator.arm.name + ".srdf"
with open(arm_srdf_path, 'r') as stream:
self.arm_srdf_xml = parse(stream)
xacro.process_includes(self.arm_srdf_xml,
os.path.dirname(sys.argv[0]))
xacro.process_doc(self.arm_srdf_xml)
if manipulator.has_hand:
# Generate and read hand srdf
hand_urdf_path = self.rospack.get_path(
'sr_description') + "/robots/" + manipulator.hand.name
with open(hand_urdf_path, 'r') as hand_urdf_xacro_file:
hand_urdf_xml = parse(hand_urdf_xacro_file)
xacro.process_includes(hand_urdf_xml,
os.path.dirname(sys.argv[0]))
xacro.process_doc(hand_urdf_xml)
hand_urdf = hand_urdf_xml.toprettyxml(indent=' ')
srdfHandGenerator = SRDFHandGenerator(hand_urdf,
load=False,
save=False)
self.hand_srdf_xml = srdfHandGenerator.get_hand_srdf()
comment = ["Manipulator:" + manipulator.name]
self.add_comments(comment)
# Add groups and group states
if manipulator.has_arm:
self.parse_arm_groups(manipulator)
if manipulator.has_hand:
self.parse_hand_groups(manipulator)
# Add end effectors
comment = [
"END EFFECTOR: Purpose: Represent information about an end effector."
]
self.add_comments(comment)
if manipulator.has_hand:
self.parse_hand_end_effectors(manipulator)
if manipulator.has_arm:
self.parse_arm_end_effectors(manipulator)
# Add virtual joints
if manipulator.has_arm:
pass
# self.parse_arm_virtual_joint(manipulator)
else:
self.parse_hand_virtual_joint(manipulator)
# Add disable collisions
comment = [
"DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially "
+
"come into collision with any other link in the robot. This tag disables collision checking "
+ "between a specified pair of links."
]
self.add_comments(comment)
if manipulator.has_arm:
self.parse_arm_collisions(manipulator)
if manipulator.has_hand:
self.parse_hand_collisions(manipulator)
# Finish and close file
self.new_robot_srdf.write('</robot>\n')
self.new_robot_srdf.close()
if load:
with open(self.package_path + "/config/" + new_srdf_file_name,
'r') as stream:
srdf = parse(stream)
rospy.loginfo("Loading SRDF on parameter server")
robot_description_param = rospy.resolve_name(
'robot_description') + "_semantic"
rospy.set_param(robot_description_param,
srdf.toprettyxml(indent=' '))
if self._save_files:
rospy.loginfo("Robot urdf and srdf have been saved to %s" %
self._path_to_save_files)
# srdf: File is already generated so just need to be copied to specified location
copy2(self.package_path + "/config/" + new_srdf_file_name,
self._path_to_save_files + "/" + self._file_name + ".srdf")
# urdf: File can be copied from rosparam
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_file = open(
self._path_to_save_files + "/" + self._file_name + ".urdf",
"wb")
urdf_file.write(urdf_str)
urdf_file.close()
rospy.loginfo("generated_robot.srdf has been generated and saved.")
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
package_name = "gcamp_gazebo"
robot_file = "skidbot.urdf"
rviz_file = "test.rviz"
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
os.path.join(get_package_share_directory("gazebo_ros"), "launch"),
"/gazebo.launch.py",
]
),
)
urdf = os.path.join(get_package_share_directory(package_name), "urdf", robot_file)
rviz = os.path.join(get_package_share_directory(package_name), "rviz", rviz_file)
xml = open(urdf, "r").read()
xml = xml.replace('"', '\\"')
xacro_file = os.path.join(
get_package_share_directory(package_name), "urdf", robot_file
)
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
params = {"robot_description": doc.toxml()}
# node_robot_state_publisher = Node(
# package="robot_state_publisher",
# executable="robot_state_publisher",
# output="screen",
# parameters=[params],
# )
with open(urdf, 'r') as infp:
robot_desc = infp.read()
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
arguments=[urdf],
# parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
)
joint_state_publisher_node = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
spawn_entity = Node(
package="gazebo_ros",
executable="spawn_entity.py",
arguments=["-topic", "robot_description", "-entity"],
output="screen",
)
return LaunchDescription(
[
DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
DeclareLaunchArgument(name='rvizconfig', default_value=rviz,
description='Absolute path to rviz config file'),
DeclareLaunchArgument(name='model', default_value=urdf,
description='Absolute path to robot urdf file'),
robot_state_publisher_node,
joint_state_publisher_node,
joint_state_publisher_gui_node,
rviz_node,
# gazebo,
# spawn_entity,
]
)
def xacro_parse(filename):
doc = xacro.parse(None, filename)
xacro.process_doc(doc, in_order=True)
return doc.toprettyxml(indent=' ')
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
)
xacro_file = os.path.join(get_package_share_directory('turtlebot3_manipulation_gazebo'),
'urdf',
'turtlebot3_pi_manipulator.xacro')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
params = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'omx'], # TODO - 2) change entity INSTANCE name
# (if spawning multiple robots, each -entity needs to be unique)
output='screen')
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'],
output='screen'
)
load_joint_trajectory_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'],
output='screen'
)
# TODO 3): if adding effort or velocity controllers, add here as above: these depend on ros2-control and
# ros2_controllers
# load_velocity_controller = ExecuteProcess(
# cmd=['ros2', 'control', 'load_start_controller', 'velocity_controller'],
# output='screen'
# )
load_effort_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_start_controller', 'effort_controllers'],
output='screen'
)
return LaunchDescription([
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller, load_effort_controller],
)
),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def __init__(self, urdf_str=None, load=True, save=True):
if urdf_str is None:
while not rospy.has_param('robot_description'):
rospy.sleep(0.5)
rospy.loginfo("waiting for robot_description")
# load the urdf from the parameter server
urdf_str = rospy.get_param('robot_description')
robot = URDF.from_xml_string(urdf_str)
extracted_prefix = False
prefix = ""
ff = mf = rf = lf = th = False
is_lite = True
is_biotac = False
hand_name = "right_hand"
# Check if hand has the old biotac sensors
for key in robot.link_map:
link = robot.link_map[key]
if link.visual:
if hasattr(link.visual.geometry, 'filename'):
filename = os.path.basename(link.visual.geometry.filename)
if filename == "biotac_decimated.dae":
is_biotac = True
break
for key in robot.joint_map:
# any joint is supposed to have the same prefix and a joint name with 4 chars
if not extracted_prefix:
prefix = key.split("_")[0] + "_"
rospy.loginfo("Found prefix:" + prefix)
extracted_prefix = True
if prefix == "lh_":
hand_name = "left_hand"
if not ff and key.endswith("FFJ4"):
ff = True
if not mf and key.endswith("MFJ4"):
mf = True
if not rf and key.endswith("RFJ4"):
rf = True
if not lf and key.endswith("LFJ4"):
lf = True
if not th and key.endswith("THJ4"):
th = True
if is_lite and key.endswith("WRJ2"):
is_lite = False
rospy.logdebug("Found fingers (ff mf rf lf th)" + str(ff) + str(mf) +
str(rf) + str(lf) + str(th))
rospy.logdebug("is_lite: " + str(is_lite))
rospy.logdebug("is_biotac: " + str(is_biotac))
rospy.logdebug("Hand name: " + str(hand_name))
mappings = load_mappings([
'prefix:=' + str(prefix), 'robot_name:=' + robot.name,
'ff:=' + str(int(ff)), 'mf:=' + str(int(mf)),
'rf:=' + str(int(rf)), 'lf:=' + str(int(lf)),
'th:=' + str(int(th)), 'is_lite:=' + str(int(is_lite)),
'is_biotac:=' + str(int(is_biotac)), 'hand_name:=' + str(hand_name)
])
# the prefix version of the srdf_xacro must be loaded
rospack = rospkg.RosPack()
package_path = rospack.get_path('sr_moveit_hand_config')
srdf_xacro_filename = package_path + "/config/shadowhands_prefix.srdf.xacro"
rospy.loginfo("File loaded " + srdf_xacro_filename)
# open and parse the xacro.srdf file
srdf_xacro_file = open(srdf_xacro_filename, 'r')
self.srdf_xacro_xml = parse(srdf_xacro_file)
# expand the xacro
xacro.process_includes(self.srdf_xacro_xml,
os.path.dirname(sys.argv[0]))
xacro.process_doc(self.srdf_xacro_xml, mappings=mappings)
if len(sys.argv) > 1:
OUTPUT_PATH = sys.argv[1]
# reject ROS internal parameters and detect termination
if (OUTPUT_PATH.startswith("_") or OUTPUT_PATH.startswith("--")):
OUTPUT_PATH = None
else:
OUTPUT_PATH = None
if load:
rospy.loginfo("Loading SRDF on parameter server")
robot_description_param = rospy.resolve_name(
'robot_description') + "_semantic"
rospy.set_param(robot_description_param,
self.srdf_xacro_xml.toprettyxml(indent=' '))
if save:
OUTPUT_PATH = package_path + "/config/generated_shadowhand.srdf"
FW = open(OUTPUT_PATH, "wb")
FW.write(self.srdf_xacro_xml.toprettyxml(indent=' '))
FW.close()
OUTPUT_PATH = package_path + "/config/generated_shadowhand.urdf"
FW = open(OUTPUT_PATH, "wb")
FW.write(urdf_str)
FW.close()
srdf_xacro_file.close()
def xacro(template_xml, **kwargs):
doc = parse(template_xml)
process_doc(doc, **kwargs)
xml = doc.toprettyxml(indent=' ')
xml = xml.replace(' xmlns:xacro="http://ros.org/wiki/xacro"', '', 1)
return xml
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
rviz_file = "skidbot.rviz"
robot_file = "skidbot.urdf"
package_name = "gcamp_gazebo"
world_file_name = "gcamp_world.world"
pkg_path = os.path.join(get_package_share_directory(package_name))
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
urdf_file = os.path.join(pkg_path, "urdf", robot_file)
rviz_config = os.path.join(pkg_path, "rviz", rviz_file)
world_path = os.path.join(pkg_path, "worlds", world_file_name)
# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
launch_arguments={'world': world_path}.items()
)
# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
)
# Robot State Publisher
doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]
)
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
output='screen',
arguments=['-topic', 'robot_description', '-entity', 'skidbot'],
)
rviz_start = ExecuteProcess(
cmd=["ros2", "run", "rviz2", "rviz2", "-d", rviz_config], output="screen"
)
# create and return launch description object
return LaunchDescription(
[
TimerAction(
period=3.0,
actions=[rviz_start]
),
# start gazebo, notice we are using libgazebo_ros_factory.so instead of libgazebo_ros_init.so
# That is because only libgazebo_ros_factory.so contains the service call to /spawn_entity
# ExecuteProcess(
# cmd=["gazebo", "--verbose", world_path, "-s", "libgazebo_ros_factory.so"],
# output="screen",
# ),
start_gazebo_server_cmd,
start_gazebo_client_cmd,
robot_state_publisher_node,
# tell gazebo to spwan your robot in the world by calling service
spawn_entity,
]
)
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
# NOTE: Load the processed xacro file directly
description_location = os.path.join(
get_package_share_directory('racecar_description'))
xacro_file = os.path.join(description_location, 'urdf', 'racecar.xacro')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
robot_description_value = doc.toxml()
params = {"robot_description": robot_description_value}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params])
spawn_entity = Node(package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-topic', 'robot_description', '-entity',
'racecar', '-x', '0', '-y', '0', '-z', '0.5'
],
output='screen')
# NOTE: THIS IS DEPRECATED VERSION OF LOADING
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_start_controller', 'joint_state_broadcaster'
],
output='screen')
# NOTE: THIS IS DEPRECATED VERSION OF LOADING CONTROLLER
controllers = [
'left_rear_wheel_velocity_controller',
'right_rear_wheel_velocity_controller',
'left_front_wheel_velocity_controller',
'right_front_wheel_velocity_controller',
'left_steering_hinge_position_controller',
'right_steering_hinge_position_controller'
]
load_controllers = [
ExecuteProcess(cmd=['ros2', 'control', 'load_start_controller', state],
output='screen') for state in controllers
]
racecar_controller = os.path.join(
get_package_share_directory("racecar_control"),
"config",
"racecar_control.yaml",
)
return LaunchDescription([
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=load_controllers,
)),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
world_file_name = 'warehouse.world'
pkg_dir = get_package_share_directory('orthopillar_robot_spawner_pkg')
default_rviz_config_path = os.path.join(pkg_dir, 'rviz/config.rviz')
os.environ["GAZEBO_MODEL_PATH"] = os.path.join(pkg_dir, 'models')
world = os.path.join(pkg_dir, 'worlds', world_file_name)
launch_file_dir = os.path.join(pkg_dir, 'launch')
urdf_file = os.path.join(pkg_dir, 'models/orthopillar', 'model.urdf')
doc = xacro.parse(open(urdf_file))
xacro.process_doc(doc)
robot_description = {'robot_description': doc.toxml()}
#urdf_file_name = 'urdf/model.urdf.xml'
#urdf = os.path.join(
# get_package_share_directory('orthopillar_robot_spawner_pkg'),
# urdf_file_name)
#with open(urdf, 'r') as infp:
# robot_description = infp.read()
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description])
joint_state_publisher_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
#condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = launch_ros.actions.Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
#condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)
rviz_node = launch_ros.actions.Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
gazebo = ExecuteProcess(cmd=[
'gazebo', '--verbose', world, '-s', 'libgazebo_ros_init.so', '-s',
'libgazebo_ros_factory.so'
],
output='screen')
#GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model
#spawn_entity = Node(package='gazebo_ros', node_executable='spawn_entity.py',
# arguments=['-entity', 'demo', 'x', 'y', 'z'],
# output='screen')
spawn_entity = Node(
package='orthopillar_robot_spawner_pkg',
executable='spawn_demo',
arguments=['orthopillar', 'demo', '-1.5', '-4.0', '0.0'],
output='screen')
#robot_state_publisher = Node( package='robot_state_publisher',
# executable='robot_state_publisher',
# name='robot_state_publisher',
# output='screen',
# parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_description}],
# arguments=[urdf])
#state_publisher = Node( package='orthopillar_robot_spawner_pkg',
# executable='state_publisher',
# name='state_publisher',
# output='screen')
return launch.LaunchDescription([
launch.actions.DeclareLaunchArgument(
name='rvizconfig',
default_value=default_rviz_config_path,
description='Absolute path to rviz config file'),
gazebo,
spawn_entity,
robot_state_publisher_node,
joint_state_publisher_node,
joint_state_publisher_gui_node,
#state_publisher
rviz_node
])
