mirror of
https://github.com/bjoernellens1/cps_rmp220_support.git
synced 2025-01-19 08:06:59 +00:00
50 lines
1.6 KiB
Python
50 lines
1.6 KiB
Python
|
import os
|
||
|
|
import rospy
|
||
|
|
import rospkg
|
||
|
|
import xacro
|
||
|
|
from xml.etree.ElementTree import Element, tostring
|
||
|
|
|
||
|
|
def generate_launch_description():
|
||
|
|
# Initialize ROS node
|
||
|
|
rospy.init_node('robot_description_launch', anonymous=True)
|
||
|
|
|
||
|
|
# Get the package path
|
||
|
|
rospack = rospkg.RosPack()
|
||
|
|
pkg_path = rospack.get_path('cps_rmp220_support')
|
||
|
|
xacro_file = os.path.join(pkg_path, 'description', 'robot.urdf.xacro')
|
||
|
|
|
||
|
|
# Read parameters
|
||
|
|
use_sim_time = rospy.get_param('~use_sim_time', False)
|
||
|
|
use_ros_control = rospy.get_param('~use_ros_control', True)
|
||
|
|
|
||
|
|
# Process the URDF file using xacro
|
||
|
|
try:
|
||
|
|
urdf_tree = xacro.process_file(
|
||
|
|
xacro_file,
|
||
|
|
mappings={
|
||
|
|
'use_ros_control': str(use_ros_control).lower(),
|
||
|
|
'sim_mode': str(use_sim_time).lower(),
|
||
|
|
}
|
||
|
|
)
|
||
|
|
robot_description = tostring(urdf_tree.getroot(), encoding='unicode')
|
||
|
|
except Exception as e:
|
||
|
|
rospy.logerr(f"Error processing xacro file: {e}")
|
||
|
|
robot_description = ""
|
||
|
|
|
||
|
|
# Set the parameter for the robot description
|
||
|
|
rospy.set_param('/robot_description', robot_description)
|
||
|
|
rospy.set_param('/use_sim_time', use_sim_time)
|
||
|
|
|
||
|
|
# Launch nodes
|
||
|
|
state_publisher_command = "rosrun robot_state_publisher robot_state_publisher"
|
||
|
|
joint_state_publisher_command = "rosrun joint_state_publisher joint_state_publisher"
|
||
|
|
|
||
|
|
os.system(state_publisher_command)
|
||
|
|
os.system(joint_state_publisher_command)
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
try:
|
||
|
|
generate_launch_description()
|
||
|
|
except rospy.ROSInterruptException:
|
||
|
|
pass
|