diff --git a/launch/robot_scan_filter.launch b/launch/robot_scan_filter.launch new file mode 100644 index 0000000..863e304 --- /dev/null +++ b/launch/robot_scan_filter.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/rsp.launch.py b/launch/rsp.launch.py new file mode 100644 index 0000000..56d686b --- /dev/null +++ b/launch/rsp.launch.py @@ -0,0 +1,49 @@ +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