From 0403aba97b87d18860abbd581842db5e0f85d4be Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Fri, 18 Mar 2022 09:16:59 +0100 Subject: [PATCH] improved iisy_config module --- .gitignore | 1 + src/gaz_simulation/CMakeLists.txt | 2 - .../launch/.moveit_launch.py.kate-swp | Bin 169 -> 0 bytes src/gaz_simulation/launch/moveit_launch.py | 64 +-- src/iisy_config/config/config.rviz | 384 ++++++++++++++++++ src/iisy_config/config/iisy_controllers.yml | 42 ++ .../config/iisy_moveit_controllers.yml | 15 + .../{kinematics.yaml => kinematics.yml} | 0 .../{ompl_planning.yaml => ompl_planning.yml} | 0 .../launch/chomp_planning_pipeline.launch.xml | 23 -- .../launch/default_warehouse_db.launch | 15 - src/iisy_config/launch/demo.launch | 68 ---- src/iisy_config/launch/demo_gazebo.launch | 21 - .../fake_moveit_controller_manager.launch.xml | 12 - src/iisy_config/launch/gazebo.launch | 32 -- .../iisy_moveit_sensor_manager.launch.xml | 3 - .../launch/joystick_control.launch | 17 - src/iisy_config/launch/lbr_bringup.launch.py | 152 +++++++ src/iisy_config/launch/lbr_control.launch.py | 86 ++++ .../launch/lbr_move_group.launch.py | 181 +++++++++ .../launch/lbr_simulation.launch.py | 49 +++ src/iisy_config/launch/move_group.launch | 101 ----- src/iisy_config/launch/moveit_rviz.launch | 15 - .../ompl-chomp_planning_pipeline.launch.xml | 19 - .../launch/ompl_planning_pipeline.launch.xml | 26 -- ...otion_planner_planning_pipeline.launch.xml | 15 - .../launch/planning_context.launch | 26 -- .../launch/planning_pipeline.launch.xml | 10 - ...ntrol_moveit_controller_manager.launch.xml | 4 - src/iisy_config/launch/ros_controllers.launch | 11 - .../launch/run_benchmark_ompl.launch | 21 - .../launch/sensor_manager.launch.xml | 17 - src/iisy_config/launch/setup_assistant.launch | 16 - ...imple_moveit_controller_manager.launch.xml | 8 - .../launch/stomp_planning_pipeline.launch.xml | 25 -- .../launch/trajectory_execution.launch.xml | 23 -- src/iisy_config/launch/view_robot.launch.py | 93 +++++ src/iisy_config/launch/warehouse.launch | 15 - .../launch/warehouse_settings.launch.xml | 16 - src/iisy_config/{config => srdf}/iisy.srdf | 0 .../stl/base_bottom_coll.stl | Bin .../stl/base_bottom_low.stl | Bin .../stl/base_top_low.stl | Bin .../stl/link_1_full_coll.stl | Bin .../stl/link_1_full_low.stl | Bin .../stl/link_2_bottom_low.stl | Bin .../stl/link_2_top_coll.stl | Bin .../stl/link_2_top_low.stl | Bin .../stl/link_3_bottom_coll.stl | Bin .../stl/link_3_bottom_low.stl | Bin .../stl/link_3_top_coll.stl | Bin .../stl/link_3_top_low.stl | Bin .../urdf/iisy.urdf | 181 ++++++++- src/moveit2_tutorials | 1 - src/moveit_visual_tools | 1 - 55 files changed, 1203 insertions(+), 608 deletions(-) delete mode 100644 src/gaz_simulation/launch/.moveit_launch.py.kate-swp create mode 100644 src/iisy_config/config/config.rviz create mode 100644 src/iisy_config/config/iisy_controllers.yml create mode 100644 src/iisy_config/config/iisy_moveit_controllers.yml rename src/iisy_config/config/{kinematics.yaml => kinematics.yml} (100%) rename src/iisy_config/config/{ompl_planning.yaml => ompl_planning.yml} (100%) delete mode 100644 src/iisy_config/launch/chomp_planning_pipeline.launch.xml delete mode 100644 src/iisy_config/launch/default_warehouse_db.launch delete mode 100644 src/iisy_config/launch/demo.launch delete mode 100644 src/iisy_config/launch/demo_gazebo.launch delete mode 100644 src/iisy_config/launch/fake_moveit_controller_manager.launch.xml delete mode 100644 src/iisy_config/launch/gazebo.launch delete mode 100644 src/iisy_config/launch/iisy_moveit_sensor_manager.launch.xml delete mode 100644 src/iisy_config/launch/joystick_control.launch create mode 100644 src/iisy_config/launch/lbr_bringup.launch.py create mode 100644 src/iisy_config/launch/lbr_control.launch.py create mode 100644 src/iisy_config/launch/lbr_move_group.launch.py create mode 100644 src/iisy_config/launch/lbr_simulation.launch.py delete mode 100644 src/iisy_config/launch/move_group.launch delete mode 100644 src/iisy_config/launch/moveit_rviz.launch delete mode 100644 src/iisy_config/launch/ompl-chomp_planning_pipeline.launch.xml delete mode 100644 src/iisy_config/launch/ompl_planning_pipeline.launch.xml delete mode 100644 src/iisy_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml delete mode 100644 src/iisy_config/launch/planning_context.launch delete mode 100644 src/iisy_config/launch/planning_pipeline.launch.xml delete mode 100644 src/iisy_config/launch/ros_control_moveit_controller_manager.launch.xml delete mode 100644 src/iisy_config/launch/ros_controllers.launch delete mode 100644 src/iisy_config/launch/run_benchmark_ompl.launch delete mode 100644 src/iisy_config/launch/sensor_manager.launch.xml delete mode 100644 src/iisy_config/launch/setup_assistant.launch delete mode 100644 src/iisy_config/launch/simple_moveit_controller_manager.launch.xml delete mode 100644 src/iisy_config/launch/stomp_planning_pipeline.launch.xml delete mode 100644 src/iisy_config/launch/trajectory_execution.launch.xml create mode 100644 src/iisy_config/launch/view_robot.launch.py delete mode 100644 src/iisy_config/launch/warehouse.launch delete mode 100644 src/iisy_config/launch/warehouse_settings.launch.xml rename src/iisy_config/{config => srdf}/iisy.srdf (100%) rename src/{gaz_simulation => iisy_config}/stl/base_bottom_coll.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/base_bottom_low.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/base_top_low.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_1_full_coll.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_1_full_low.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_2_bottom_low.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_2_top_coll.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_2_top_low.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_3_bottom_coll.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_3_bottom_low.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_3_top_coll.stl (100%) rename src/{gaz_simulation => iisy_config}/stl/link_3_top_low.stl (100%) rename src/{gaz_simulation => iisy_config}/urdf/iisy.urdf (54%) delete mode 160000 src/moveit2_tutorials delete mode 160000 src/moveit_visual_tools diff --git a/.gitignore b/.gitignore index c32553f..cfee5e7 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ AMENT_IGNORE # End of https://www.toptal.com/developers/gitignore/api/ros2 +.vscode \ No newline at end of file diff --git a/src/gaz_simulation/CMakeLists.txt b/src/gaz_simulation/CMakeLists.txt index 58a02eb..707c7f9 100644 --- a/src/gaz_simulation/CMakeLists.txt +++ b/src/gaz_simulation/CMakeLists.txt @@ -13,8 +13,6 @@ find_package(ament_cmake REQUIRED) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY world DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY stl DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/gaz_simulation/launch/.moveit_launch.py.kate-swp b/src/gaz_simulation/launch/.moveit_launch.py.kate-swp deleted file mode 100644 index de3a6aba4bfd75973b77bde10e65829233d4ab66..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 169 zcmZQzU=Z?7EJ;-eE>A2_aLdd|RWQ;sU|?VnS@~1rMv(L2CD;4HUq1LDZ{A_%6&xG{ zl<@;%5a0%Je8Efx24xV(2g;EKalD}%MG(ge%25GvJfR%^P$0t(MEHUT9}wXUBD_GV OJb;)3h`B)`o~{5q?itwt diff --git a/src/gaz_simulation/launch/moveit_launch.py b/src/gaz_simulation/launch/moveit_launch.py index ba77ec6..cf9d657 100644 --- a/src/gaz_simulation/launch/moveit_launch.py +++ b/src/gaz_simulation/launch/moveit_launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable from launch_ros.substitutions import FindPackageShare -from launch.substitutions import Command +from launch.substitutions import Command, LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory @@ -118,18 +118,15 @@ def generate_launch_description(): output='screen' ), - Node( - package='gazebo_ros', - executable='spawn_entity.py', - arguments=['-entity', robot_name, - '-topic', 'robot_description', - '-x', '-1', - '-y', '-1', - '-z', '1', - '-Y', '0'], # Yaw - output='screen' - ), - + #Node( + # package="gazebo_ros", + # executable="spawn_entity.py", + # arguments=[ + # "-topic", "robot_description", + # "-entity", LaunchConfiguration("iisy") + # ], + # output="screen" + #), #Node( # package='robot_state_publisher', @@ -159,21 +156,30 @@ def generate_launch_description(): # arguments=["test.xml"] # ) + Node( + package='controller_manager', + name='gazebo_controller_spawner', + executable='spawner', + output='screen', + parameters=[ + controllers_yaml, + ] + ), - #Node(package='moveit_ros_move_group', - # executable='move_group', - # #prefix='xterm -fs 10 -e gdb --ex run --args', - # output='screen', - # parameters=[ - # robot_description, - # robot_description_semantic, - # kinematics_yaml, - # ompl_planning_pipeline_config, - # trajectory_execution, - # moveit_controllers, - # planning_scene_monitor_parameters, - # #octomap_config, - # #octomap_updater_config - # ] - #) + Node(package='moveit_ros_move_group', + executable='move_group', + #prefix='xterm -fs 10 -e gdb --ex run --args', + output='screen', + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + #octomap_config, + #octomap_updater_config + ] + ), ]) \ No newline at end of file diff --git a/src/iisy_config/config/config.rviz b/src/iisy_config/config/config.rviz new file mode 100644 index 0000000..1be2d1c --- /dev/null +++ b/src/iisy_config/config/config.rviz @@ -0,0 +1,384 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 212 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + lbr_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + lbr_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: med7_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + lbr_link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lbr_link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.01 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.398728847503662 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.056836556643247604 + Y: -0.0032705869525671005 + Z: 0.7173376083374023 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5103980898857117 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.248579978942871 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f30000032afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000111000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000154000002130000017d00ffffff000000010000010000000280fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000280000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000006efc0100000002fb0000000800540069006d00650100000000000007380000005800fffffffb0000000800540069006d006501000000000000045000000000000000000000053f0000032a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 0 + Y: 27 diff --git a/src/iisy_config/config/iisy_controllers.yml b/src/iisy_config/config/iisy_controllers.yml new file mode 100644 index 0000000..84195fa --- /dev/null +++ b/src/iisy_config/config/iisy_controllers.yml @@ -0,0 +1,42 @@ +# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41 + + position_trajectory_controller: + # find declaration here https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_trajectory_controller/joint_trajectory_plugin.xml#L2 + type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + +position_trajectory_controller: + ros__parameters: + # find required parameters here https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot + command_interfaces: + - position + state_interfaces: + - position + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + +forward_position_controller: + ros__parameters: + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot + interface_name: "position" diff --git a/src/iisy_config/config/iisy_moveit_controllers.yml b/src/iisy_config/config/iisy_moveit_controllers.yml new file mode 100644 index 0000000..7e8cc98 --- /dev/null +++ b/src/iisy_config/config/iisy_moveit_controllers.yml @@ -0,0 +1,15 @@ +controller_names: + - position_trajectory_controller + +# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller +position_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot diff --git a/src/iisy_config/config/kinematics.yaml b/src/iisy_config/config/kinematics.yml similarity index 100% rename from src/iisy_config/config/kinematics.yaml rename to src/iisy_config/config/kinematics.yml diff --git a/src/iisy_config/config/ompl_planning.yaml b/src/iisy_config/config/ompl_planning.yml similarity index 100% rename from src/iisy_config/config/ompl_planning.yaml rename to src/iisy_config/config/ompl_planning.yml diff --git a/src/iisy_config/launch/chomp_planning_pipeline.launch.xml b/src/iisy_config/launch/chomp_planning_pipeline.launch.xml deleted file mode 100644 index 33543cb..0000000 --- a/src/iisy_config/launch/chomp_planning_pipeline.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/default_warehouse_db.launch b/src/iisy_config/launch/default_warehouse_db.launch deleted file mode 100644 index e965b09..0000000 --- a/src/iisy_config/launch/default_warehouse_db.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/demo.launch b/src/iisy_config/launch/demo.launch deleted file mode 100644 index 4ed05b4..0000000 --- a/src/iisy_config/launch/demo.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [move_group/fake_controller_joint_states] - - - - [move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/demo_gazebo.launch b/src/iisy_config/launch/demo_gazebo.launch deleted file mode 100644 index a9f320c..0000000 --- a/src/iisy_config/launch/demo_gazebo.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/fake_moveit_controller_manager.launch.xml b/src/iisy_config/launch/fake_moveit_controller_manager.launch.xml deleted file mode 100644 index d298e6f..0000000 --- a/src/iisy_config/launch/fake_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/src/iisy_config/launch/gazebo.launch b/src/iisy_config/launch/gazebo.launch deleted file mode 100644 index 494c07f..0000000 --- a/src/iisy_config/launch/gazebo.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/iisy_moveit_sensor_manager.launch.xml b/src/iisy_config/launch/iisy_moveit_sensor_manager.launch.xml deleted file mode 100644 index 5d02698..0000000 --- a/src/iisy_config/launch/iisy_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/src/iisy_config/launch/joystick_control.launch b/src/iisy_config/launch/joystick_control.launch deleted file mode 100644 index 9411f6e..0000000 --- a/src/iisy_config/launch/joystick_control.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/lbr_bringup.launch.py b/src/iisy_config/launch/lbr_bringup.launch.py new file mode 100644 index 0000000..8f86f37 --- /dev/null +++ b/src/iisy_config/launch/lbr_bringup.launch.py @@ -0,0 +1,152 @@ +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.launch_description import LaunchDescription +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def launch_setup(context, *args, **kwargs): + + # Evaluate frequently used variables + model = LaunchConfiguration("model").perform(context) + + # Load robot description + robot_description_content = Command( + [ + FindExecutable(name="xacro"), " ", + PathJoinSubstitution( + [FindPackageShare("iisy_config"), "urdf/{}.urdf".format( model)] + ), " ", + "robot_name:=", LaunchConfiguration("robot_name"), " ", + "sim:=", LaunchConfiguration("sim") + ] + ) + + # Load controls + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("iisy_config"), + "launch", + "lbr_control.launch.py" + ]) + ), launch_arguments=[ + ("robot_description", robot_description_content), + ("controller_configurations_package", LaunchConfiguration("controller_configurations_package")), + ("controller_configurations", LaunchConfiguration("controller_configurations")), + ("controller", LaunchConfiguration("controller")), + ("sim", LaunchConfiguration("sim")) + ] + ) + + # Gazebo simulation + simulation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("iisy_config"), + "launch", + "lbr_simulation.launch.py" + ]) + ), + launch_arguments=[ + ("robot_name", LaunchConfiguration("robot_name")) + ], + condition=IfCondition(LaunchConfiguration("sim")) + ) + + # Move group + move_group = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("iisy_config"), + "launch", + "lbr_move_group.launch.py" + ]) + ), + launch_arguments=[ + ("robot_description", robot_description_content), + ("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")), + ("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")), + ("model", LaunchConfiguration("model")), + ("sim", LaunchConfiguration("sim")) + ] + ) + + return [ + simulation, + control, + move_group + ] + + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append(DeclareLaunchArgument( + name="model", + default_value="iisy", + description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14." + )) + + launch_args.append(DeclareLaunchArgument( + name="robot_name", + default_value="iisy", + description="Set robot name." + )) + + launch_args.append(DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + )) + + launch_args.append( + DeclareLaunchArgument( + name="controller_configurations_package", + default_value="iisy_config", + description="Package that contains controller configurations." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="controller_configurations", + default_value="config/iisy_controllers.yml", + description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="controller", + default_value="position_trajectory_controller", + description="Robot controller." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="moveit_controller_configurations_package", + default_value="iisy_config", + description="Package that contains MoveIt! controller configurations." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="moveit_controller_configurations", + default_value="config/iisy_moveit_controllers.yml", + description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file." + ) + ) + + return LaunchDescription( + launch_args + [ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/iisy_config/launch/lbr_control.launch.py b/src/iisy_config/launch/lbr_control.launch.py new file mode 100644 index 0000000..2a7ec59 --- /dev/null +++ b/src/iisy_config/launch/lbr_control.launch.py @@ -0,0 +1,86 @@ +from launch.launch_description import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import UnlessCondition +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append( + DeclareLaunchArgument( + name="controller_configurations_package", + default_value="iisy_config", + description="Package that contains controller configurations." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="controller_configurations", + default_value="config/iisy_controllers.yml", + description="Relative path to controller configurations YAML file." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="controller", + default_value="position_trajectory_controller", + description="Robot controller." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + ) + ) + + # Configure robot_description + robot_description = {"robot_description": LaunchConfiguration("robot_description")} + + # Load controllers from YAML configuration file + controller_configurations = PathJoinSubstitution([ + FindPackageShare(LaunchConfiguration("controller_configurations_package")), + LaunchConfiguration("controller_configurations") + ]) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description] + ) + + joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + controller = Node( + package="controller_manager", + executable="spawner", + arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"], + ) + + return LaunchDescription( + launch_args + [ + robot_state_publisher, + joint_state_broadcaster, + controller + ]) diff --git a/src/iisy_config/launch/lbr_move_group.launch.py b/src/iisy_config/launch/lbr_move_group.launch.py new file mode 100644 index 0000000..3335218 --- /dev/null +++ b/src/iisy_config/launch/lbr_move_group.launch.py @@ -0,0 +1,181 @@ +import os +import yaml + +import traceback + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from ament_index_python import get_package_share_directory + + +def load_yaml(package_name: str, file_path: str): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as f: + return yaml.safe_load(f) + except EnvironmentError as error: + print("Error loading yaml "+package_name+" | "+file_path) + traceback.print_exc() + return None + + +def load_file(package_name: str, file_path: str) -> str: + package_path = get_package_share_directory(package_name) + absolut_file_path = os.path.join(package_path, file_path) + + try: + with open(absolut_file_path, "r") as f: + return f.read() + except EnvironmentError: + print("Error loading file "+package_name+" | "+file_path) + traceback.print_exc() + return None + + +def launch_setup(context, *args, **kwargs): + + # Configure robot_description + robot_description = {"robot_description": LaunchConfiguration("robot_description")} + + # Robot semantics SRDF + robot_description_semantic = { + "robot_description_semantic": load_file("iisy_config", "srdf/iisy.srdf") + } + + # Kinematics + kinematics_yaml = load_yaml("iisy_config", "config/kinematics.yml") + + # Update group name + #kinematics_yaml["{}_arm".format(model)] = kinematics_yaml["group_name"] + #del kinematics_yaml["group_name"] + + # Joint limits + robot_description_planning = { + "robot_description_planning": PathJoinSubstitution( + [ + FindPackageShare("iisy_config"), + "config/joint_limits.yml" + ] + ) + } + + # Planning + ompl_yaml = load_yaml("iisy_config", "config/ompl_planning.yml") + + planning = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", + "start_state_max_bounds_error": 0.1 + } + } + + # Trajectory execution + trajectory_execution = {"allow_trajectory_execution": True, + "moveit_manage_controllers": True} + + # Controllers + controllers_yaml = load_yaml( + LaunchConfiguration("moveit_controller_configurations_package").perform(context), + LaunchConfiguration("moveit_controller_configurations").perform(context) + ) + + moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"} + + # Planning scene + planning_scene_monitor_parameters = {"publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True} + + # Time configuration + use_sim_time = {"use_sim_time": LaunchConfiguration("sim")} + + # Prepare move group node + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + arguments=["--ros-args"], + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + robot_description_planning, + ompl_yaml, + planning, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + use_sim_time + ] + ) + + # RViz + rviz_config = PathJoinSubstitution([FindPackageShare("iisy_config"), "config/config.rviz"]) + + rviz = Node( + package="rviz2", + executable="rviz2", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + planning, + use_sim_time + ], + arguments=[ + '-d', rviz_config + ] + ) + + return [ + move_group_node, + rviz + ] + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append( + DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="moveit_controller_configurations_package", + default_value="iisy_config", + description="Package that contains MoveIt! controller configurations." + ) + ) + + launch_args.append( + DeclareLaunchArgument( + name="moveit_controller_configurations", + default_value="config/iisy_moveit_controllers.yml", + description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name." + ) + ) + + launch_args.append(DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + )) + + return LaunchDescription( + launch_args + [ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/iisy_config/launch/lbr_simulation.launch.py b/src/iisy_config/launch/lbr_simulation.launch.py new file mode 100644 index 0000000..e8fa297 --- /dev/null +++ b/src/iisy_config/launch/lbr_simulation.launch.py @@ -0,0 +1,49 @@ +from launch.launch_description import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append(DeclareLaunchArgument( + name="robot_name", + default_value="iisy", + description="Set robot name." + )) + + # Launch Gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("gazebo_ros"), + "launch", + "gazebo.launch.py" + ]) + ) + ) + + # Note: Environment variable GAZEBO_MODEL_PATH is extended as in + # ROS2 control demos via environment hook https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description + # Also see https://colcon.readthedocs.io/en/released/developer/environment.html#dsv-files + # Gazebo launch scripts append GAZEBO_MODEL_PATH with known paths, see https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ab1ae5c05eda62674b36df74eb3be8c93cdc8761/gazebo_ros/launch/gzclient.launch.py#L26 + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=[ + "-topic", "robot_description", + "-entity", LaunchConfiguration("robot_name") + ], + output="screen" + ) + + return LaunchDescription( + launch_args + [ + gazebo, + spawn_entity + ]) diff --git a/src/iisy_config/launch/move_group.launch b/src/iisy_config/launch/move_group.launch deleted file mode 100644 index e082c0c..0000000 --- a/src/iisy_config/launch/move_group.launch +++ /dev/null @@ -1,101 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/moveit_rviz.launch b/src/iisy_config/launch/moveit_rviz.launch deleted file mode 100644 index a4605c0..0000000 --- a/src/iisy_config/launch/moveit_rviz.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/ompl-chomp_planning_pipeline.launch.xml b/src/iisy_config/launch/ompl-chomp_planning_pipeline.launch.xml deleted file mode 100644 index b078992..0000000 --- a/src/iisy_config/launch/ompl-chomp_planning_pipeline.launch.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - diff --git a/src/iisy_config/launch/ompl_planning_pipeline.launch.xml b/src/iisy_config/launch/ompl_planning_pipeline.launch.xml deleted file mode 100644 index 2348490..0000000 --- a/src/iisy_config/launch/ompl_planning_pipeline.launch.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/src/iisy_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml deleted file mode 100644 index c7c4cf5..0000000 --- a/src/iisy_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/planning_context.launch b/src/iisy_config/launch/planning_context.launch deleted file mode 100644 index cf28d5f..0000000 --- a/src/iisy_config/launch/planning_context.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/planning_pipeline.launch.xml b/src/iisy_config/launch/planning_pipeline.launch.xml deleted file mode 100644 index 4b4d0d6..0000000 --- a/src/iisy_config/launch/planning_pipeline.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/src/iisy_config/launch/ros_control_moveit_controller_manager.launch.xml b/src/iisy_config/launch/ros_control_moveit_controller_manager.launch.xml deleted file mode 100644 index 9ebc91c..0000000 --- a/src/iisy_config/launch/ros_control_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/src/iisy_config/launch/ros_controllers.launch b/src/iisy_config/launch/ros_controllers.launch deleted file mode 100644 index 2decb91..0000000 --- a/src/iisy_config/launch/ros_controllers.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/src/iisy_config/launch/run_benchmark_ompl.launch b/src/iisy_config/launch/run_benchmark_ompl.launch deleted file mode 100644 index 7b49cf7..0000000 --- a/src/iisy_config/launch/run_benchmark_ompl.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/sensor_manager.launch.xml b/src/iisy_config/launch/sensor_manager.launch.xml deleted file mode 100644 index e91b5c3..0000000 --- a/src/iisy_config/launch/sensor_manager.launch.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/setup_assistant.launch b/src/iisy_config/launch/setup_assistant.launch deleted file mode 100644 index 700a33b..0000000 --- a/src/iisy_config/launch/setup_assistant.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - diff --git a/src/iisy_config/launch/simple_moveit_controller_manager.launch.xml b/src/iisy_config/launch/simple_moveit_controller_manager.launch.xml deleted file mode 100644 index 3a962b2..0000000 --- a/src/iisy_config/launch/simple_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/src/iisy_config/launch/stomp_planning_pipeline.launch.xml b/src/iisy_config/launch/stomp_planning_pipeline.launch.xml deleted file mode 100644 index 805ad25..0000000 --- a/src/iisy_config/launch/stomp_planning_pipeline.launch.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/trajectory_execution.launch.xml b/src/iisy_config/launch/trajectory_execution.launch.xml deleted file mode 100644 index 20c3dfc..0000000 --- a/src/iisy_config/launch/trajectory_execution.launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/view_robot.launch.py b/src/iisy_config/launch/view_robot.launch.py new file mode 100644 index 0000000..c9e6c42 --- /dev/null +++ b/src/iisy_config/launch/view_robot.launch.py @@ -0,0 +1,93 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +# for reference see +# https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description +def generate_launch_description(): + + # Launch arguments + launch_args = [] + + launch_args.append(DeclareLaunchArgument( + name='description_package', + default_value='iisy_config', + description='Description package.' + )) + + launch_args.append(DeclareLaunchArgument( + name='description_file', + default_value='urdf/iisy.urdf', + description='Path to URDF file, relative to description_package.' + )) + + launch_args.append(DeclareLaunchArgument( + name='rviz_config', + default_value='config/config.rviz', + description='Rviz configuration relative to description_package.' + )) + + launch_args.append(DeclareLaunchArgument( + name='robot_name', + default_value='iisy', + description='Set robot name.' + )) + + launch_args.append(DeclareLaunchArgument( + name='origin_xyz', + default_value="'0 0 0'", + description='Set position origin of robot.' + )) + + launch_args.append(DeclareLaunchArgument( + name='origin_rpy', + default_value="'0 0 0'", + description='Set orientation origin of robot.' + )) + + # Load robot description + robot_description_content = Command( + [ + FindExecutable(name="xacro"), " ", + PathJoinSubstitution( + [FindPackageShare(LaunchConfiguration('description_package')), LaunchConfiguration('description_file')] + ), " ", + "origin_xyz:=", LaunchConfiguration('origin_xyz'), " ", + "origin_rpy:=", LaunchConfiguration('origin_rpy'), " ", + "robot_name:=", LaunchConfiguration('robot_name') + ] + ) + + robot_description = {'robot_description': robot_description_content} + + # Create required nodes + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz = Node( + package='rviz2', + executable='rviz2', + parameters=[{'config': PathJoinSubstitution( + [FindPackageShare(LaunchConfiguration('description_package')), LaunchConfiguration('rviz_config')] + )}] + ) + + return LaunchDescription( + launch_args + + [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz + ] + ) diff --git a/src/iisy_config/launch/warehouse.launch b/src/iisy_config/launch/warehouse.launch deleted file mode 100644 index 0712e67..0000000 --- a/src/iisy_config/launch/warehouse.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/src/iisy_config/launch/warehouse_settings.launch.xml b/src/iisy_config/launch/warehouse_settings.launch.xml deleted file mode 100644 index e473b08..0000000 --- a/src/iisy_config/launch/warehouse_settings.launch.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/src/iisy_config/config/iisy.srdf b/src/iisy_config/srdf/iisy.srdf similarity index 100% rename from src/iisy_config/config/iisy.srdf rename to src/iisy_config/srdf/iisy.srdf diff --git a/src/gaz_simulation/stl/base_bottom_coll.stl b/src/iisy_config/stl/base_bottom_coll.stl similarity index 100% rename from src/gaz_simulation/stl/base_bottom_coll.stl rename to src/iisy_config/stl/base_bottom_coll.stl diff --git a/src/gaz_simulation/stl/base_bottom_low.stl b/src/iisy_config/stl/base_bottom_low.stl similarity index 100% rename from src/gaz_simulation/stl/base_bottom_low.stl rename to src/iisy_config/stl/base_bottom_low.stl diff --git a/src/gaz_simulation/stl/base_top_low.stl b/src/iisy_config/stl/base_top_low.stl similarity index 100% rename from src/gaz_simulation/stl/base_top_low.stl rename to src/iisy_config/stl/base_top_low.stl diff --git a/src/gaz_simulation/stl/link_1_full_coll.stl b/src/iisy_config/stl/link_1_full_coll.stl similarity index 100% rename from src/gaz_simulation/stl/link_1_full_coll.stl rename to src/iisy_config/stl/link_1_full_coll.stl diff --git a/src/gaz_simulation/stl/link_1_full_low.stl b/src/iisy_config/stl/link_1_full_low.stl similarity index 100% rename from src/gaz_simulation/stl/link_1_full_low.stl rename to src/iisy_config/stl/link_1_full_low.stl diff --git a/src/gaz_simulation/stl/link_2_bottom_low.stl b/src/iisy_config/stl/link_2_bottom_low.stl similarity index 100% rename from src/gaz_simulation/stl/link_2_bottom_low.stl rename to src/iisy_config/stl/link_2_bottom_low.stl diff --git a/src/gaz_simulation/stl/link_2_top_coll.stl b/src/iisy_config/stl/link_2_top_coll.stl similarity index 100% rename from src/gaz_simulation/stl/link_2_top_coll.stl rename to src/iisy_config/stl/link_2_top_coll.stl diff --git a/src/gaz_simulation/stl/link_2_top_low.stl b/src/iisy_config/stl/link_2_top_low.stl similarity index 100% rename from src/gaz_simulation/stl/link_2_top_low.stl rename to src/iisy_config/stl/link_2_top_low.stl diff --git a/src/gaz_simulation/stl/link_3_bottom_coll.stl b/src/iisy_config/stl/link_3_bottom_coll.stl similarity index 100% rename from src/gaz_simulation/stl/link_3_bottom_coll.stl rename to src/iisy_config/stl/link_3_bottom_coll.stl diff --git a/src/gaz_simulation/stl/link_3_bottom_low.stl b/src/iisy_config/stl/link_3_bottom_low.stl similarity index 100% rename from src/gaz_simulation/stl/link_3_bottom_low.stl rename to src/iisy_config/stl/link_3_bottom_low.stl diff --git a/src/gaz_simulation/stl/link_3_top_coll.stl b/src/iisy_config/stl/link_3_top_coll.stl similarity index 100% rename from src/gaz_simulation/stl/link_3_top_coll.stl rename to src/iisy_config/stl/link_3_top_coll.stl diff --git a/src/gaz_simulation/stl/link_3_top_low.stl b/src/iisy_config/stl/link_3_top_low.stl similarity index 100% rename from src/gaz_simulation/stl/link_3_top_low.stl rename to src/iisy_config/stl/link_3_top_low.stl diff --git a/src/gaz_simulation/urdf/iisy.urdf b/src/iisy_config/urdf/iisy.urdf similarity index 54% rename from src/gaz_simulation/urdf/iisy.urdf rename to src/iisy_config/urdf/iisy.urdf index a2b248f..c378924 100644 --- a/src/gaz_simulation/urdf/iisy.urdf +++ b/src/iisy_config/urdf/iisy.urdf @@ -1,16 +1,19 @@ - + + + + - + - + @@ -30,7 +33,7 @@ - + @@ -44,12 +47,12 @@ - + - + @@ -69,7 +72,7 @@ - + @@ -83,12 +86,12 @@ - + - + @@ -102,12 +105,12 @@ - + - + @@ -121,12 +124,12 @@ - + - + @@ -153,6 +156,7 @@ + @@ -160,6 +164,7 @@ + @@ -167,6 +172,7 @@ + @@ -174,6 +180,7 @@ + @@ -181,6 +188,7 @@ + @@ -188,9 +196,10 @@ + - + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + - + + $(find iisy_config)/config/iisy_controllers.yml /iisy + + + 0.2 + 0.2 + Gazebo/Orange + + + + 0.2 + 0.2 + Gazebo/Orange + + + + 0.2 + 0.2 + Gazebo/Orange + + + + 0.2 + 0.2 + Gazebo/Orange + + + + 0.2 + 0.2 + Gazebo/Orange + + + + 0.2 + 0.2 + Gazebo/Orange + + + + 0.2 + 0.2 + Gazebo/Grey + + \ No newline at end of file diff --git a/src/moveit2_tutorials b/src/moveit2_tutorials deleted file mode 160000 index e02236a..0000000 --- a/src/moveit2_tutorials +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e02236a8096f540c356504a5ceeb1a08212421e9 diff --git a/src/moveit_visual_tools b/src/moveit_visual_tools deleted file mode 160000 index ef16670..0000000 --- a/src/moveit_visual_tools +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb