ROS Components for hexapod_robot
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

71 linhas
2.4KB

  1. from launch import LaunchDescription
  2. from launch.actions import RegisterEventHandler
  3. from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
  4. from launch.event_handlers import OnProcessExit
  5. from launch_ros.actions import Node
  6. from launch_ros.substitutions import FindPackageShare
  7. def generate_launch_description():
  8. robot_description_content = Command([
  9. PathJoinSubstitution([FindExecutable(name="xacro")]),
  10. " ",
  11. PathJoinSubstitution(
  12. [
  13. FindPackageShare("hexapod_robot"),
  14. "config",
  15. "urdf",
  16. "hexapod_robot.urdf.xacro"
  17. ]
  18. )
  19. ]
  20. )
  21. robot_description = {"robot_description": robot_description_content}
  22. robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"])
  23. robot_state_publisher_node = Node(
  24. package = "robot_state_publisher",
  25. executable = "robot_state_publisher",
  26. parameters = [robot_description],
  27. output = "both"
  28. )
  29. control_node = Node(
  30. package = "controller_manager",
  31. executable = "ros2_control_node",
  32. parameters = [robot_description, robot_controllers],
  33. output="both"
  34. )
  35. joint_state_broadcaster_spawner = Node (
  36. package = "controller_manager",
  37. executable = "spawner",
  38. arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
  39. )
  40. joint_trajectory_controller_spawner = Node(
  41. package = "controller_manager",
  42. executable = "spawner",
  43. arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"]
  44. )
  45. delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
  46. event_handler = OnProcessExit(
  47. target_action = joint_state_broadcaster_spawner,
  48. on_exit = [joint_trajectory_controller_spawner]
  49. )
  50. )
  51. nodes = [
  52. control_node,
  53. robot_state_publisher_node,
  54. joint_state_broadcaster_spawner,
  55. delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner
  56. ]
  57. return LaunchDescription(nodes)