diff --git a/cleanup_srdf_complete.sh b/cleanup_srdf_complete.sh new file mode 100755 index 0000000..65a75c9 --- /dev/null +++ b/cleanup_srdf_complete.sh @@ -0,0 +1,133 @@ +#!/bin/bash +# Script complet pour nettoyer et corriger le fichier SRDF + +echo "đŸ§č Nettoyage complet du fichier SRDF..." + +WORKSPACE_PATH="$HOME/Robot5A-Simulation" +SRDF_FILE=$(find $WORKSPACE_PATH -name "armr5.srdf" -type f | head -1) + +if [ -z "$SRDF_FILE" ]; then + echo "❌ Fichier SRDF (armr5.srdf) non trouvĂ©!" + exit 1 +fi + +echo "📁 Fichier SRDF trouvĂ© : $SRDF_FILE" + +# Sauvegarde +BACKUP_FILE="${SRDF_FILE}.backup_complete_$(date +%Y%m%d_%H%M%S)" +cp "$SRDF_FILE" "$BACKUP_FILE" +echo "đŸ’Ÿ Sauvegarde crĂ©Ă©e : $BACKUP_FILE" + +# Compter les problĂšmes avant nettoyage +ARUCO_BEFORE=$(grep -c "aruco_plane" "$SRDF_FILE" 2>/dev/null || echo "0") +echo "📊 Lignes aruco_plane avant nettoyage : $ARUCO_BEFORE" + +# ÉTAPE 1: Supprimer toutes les rĂ©fĂ©rences aux aruco_plane +echo "đŸ—‘ïž Suppression de toutes les rĂ©fĂ©rences aruco_plane..." +sed -i '/aruco_plane/d' "$SRDF_FILE" + +# ÉTAPE 2: Ajouter les rĂšgles manquantes pour toutes les camĂ©ras +echo "đŸ“· Ajout des rĂšgles de collision pour toutes les camĂ©ras..." + +# CrĂ©er un fichier temporaire avec les nouvelles rĂšgles +TEMP_FILE=$(mktemp) + +# Copier tout sauf la balise de fermeture +sed '$d' "$SRDF_FILE" > "$TEMP_FILE" + +# Ajouter les nouvelles rĂšgles pour les camĂ©ras manquantes +cat >> "$TEMP_FILE" << 'EOF' + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +EOF + +# Remplacer le fichier original +mv "$TEMP_FILE" "$SRDF_FILE" + +# VĂ©rifications finales +ARUCO_AFTER=$(grep -c "aruco_plane" "$SRDF_FILE" 2>/dev/null || echo "0") +CAMERAS_TOTAL=$(grep -c "camera_link" "$SRDF_FILE") + +echo "" +echo "✅ Nettoyage terminĂ© !" +echo "📊 RĂ©fĂ©rences aruco_plane supprimĂ©es : $ARUCO_BEFORE" +echo "📊 RĂ©fĂ©rences aruco_plane restantes : $ARUCO_AFTER" +echo "đŸ“· Total lignes camĂ©ras : $CAMERAS_TOTAL" +echo "" +echo "🔧 Maintenant recompilez :" +echo " cd $WORKSPACE_PATH" +echo " colcon build --packages-select robot_moveit_config" +echo " source install/setup.bash" +echo " ros2 launch robot_control visual_sim.launch.py" diff --git a/src/robot_control/launch/moveit_control.launch.py b/src/robot_control/launch/moveit_control.launch.py index a0cb24e..eb1ae14 100644 --- a/src/robot_control/launch/moveit_control.launch.py +++ b/src/robot_control/launch/moveit_control.launch.py @@ -37,9 +37,7 @@ def generate_launch_description(): # MoveIt configuration using MoveItConfigsBuilder moveit_config = ( MoveItConfigsBuilder("robot_moveit_config", package_name="robot_moveit_config") - .robot_description( - file_path=xacro_file, mappings={"use_sim_time": "true"} - ) + .robot_description(file_path=xacro_file, mappings={"use_sim_time": "true"}) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") .joint_limits("config/joint_limits.yaml") diff --git a/src/robot_control/launch/moveit_control2.launch.py b/src/robot_control/launch/moveit_control2.launch.py index f1adaa4..f374caa 100644 --- a/src/robot_control/launch/moveit_control2.launch.py +++ b/src/robot_control/launch/moveit_control2.launch.py @@ -36,9 +36,7 @@ def generate_launch_description(): # MoveIt configuration using MoveItConfigsBuilder moveit_config = ( MoveItConfigsBuilder("robot_moveit_config", package_name="robot_moveit_config") - .robot_description( - file_path=xacro_file, mappings={"use_sim_time": "true"} - ) + .robot_description(file_path=xacro_file, mappings={"use_sim_time": "true"}) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") .joint_limits("config/joint_limits.yaml") diff --git a/src/robot_control/launch/moveit_control_gui.launch.py b/src/robot_control/launch/moveit_control_gui.launch.py index 55c4fc1..f8ec21e 100644 --- a/src/robot_control/launch/moveit_control_gui.launch.py +++ b/src/robot_control/launch/moveit_control_gui.launch.py @@ -37,9 +37,7 @@ def generate_launch_description(): # MoveIt configuration using MoveItConfigsBuilder moveit_config = ( MoveItConfigsBuilder("robot_moveit_config", package_name="robot_moveit_config") - .robot_description( - file_path=xacro_file, mappings={"use_sim_time": "true"} - ) + .robot_description(file_path=xacro_file, mappings={"use_sim_time": "true"}) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") .joint_limits("config/joint_limits.yaml") diff --git a/src/robot_control/launch/visual_sim.launch.py b/src/robot_control/launch/visual_sim.launch.py index eab1ad7..4acc6c1 100644 --- a/src/robot_control/launch/visual_sim.launch.py +++ b/src/robot_control/launch/visual_sim.launch.py @@ -18,8 +18,14 @@ ) from launch.event_handlers import OnProcessExit, OnProcessStart from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.conditions import IfCondition, UnlessCondition # Added for conditional execution -from launch.substitutions import LaunchConfiguration, PythonExpression # Added for launch configurations and expressions +from launch.conditions import ( + IfCondition, + UnlessCondition, +) # Added for conditional execution +from launch.substitutions import ( + LaunchConfiguration, + PythonExpression, +) # Added for launch configurations and expressions from launch_ros.actions import Node, SetParameter import xacro from moveit_configs_utils import MoveItConfigsBuilder @@ -40,13 +46,11 @@ def generate_launch_description(): # Declare the 'num_cameras' launch argument num_cameras_arg = DeclareLaunchArgument( - 'num_cameras', - default_value='1', - description='Number of cameras (1 or 2)' + "num_cameras", default_value="1", description="Number of cameras (1 or 2)" ) # Launch configuration to access 'num_cameras' argument - num_cameras = LaunchConfiguration('num_cameras') + num_cameras = LaunchConfiguration("num_cameras") # Package Directories pkg_name = "robot_description" # Name of the robot description package @@ -187,7 +191,7 @@ def generate_launch_description(): parameters=[ {"use_sim_time": True}, ], - condition=UnlessCondition(PythonExpression(['"', num_cameras, '" == "2"'])) + condition=UnlessCondition(PythonExpression(['"', num_cameras, '" == "2"'])), ) # Aruco Detector Double Node @@ -198,7 +202,7 @@ def generate_launch_description(): parameters=[ {"use_sim_time": True}, ], - condition=IfCondition(PythonExpression(['"', num_cameras, '" == "2"'])) + condition=IfCondition(PythonExpression(['"', num_cameras, '" == "2"'])), ) # Launch Visual Joint State Publisher Node @@ -214,10 +218,10 @@ def generate_launch_description(): [ num_cameras_arg, # Added the launch argument Node( - package='joint_state_publisher', - executable='joint_state_publisher', - output='screen', - parameters=[{'use_sim_time': True}] + package="joint_state_publisher", + executable="joint_state_publisher", + output="screen", + parameters=[{"use_sim_time": True}], ), SetParameter(name="use_sim_time", value=True), # Enable simulation time RegisterEventHandler( @@ -237,7 +241,7 @@ def generate_launch_description(): target_action=load_arm_controller, on_exit=[ load_gripper_controller, - move_group_node + move_group_node, ], # Load gripper controller before move group ) ), diff --git a/src/robot_description/CMakeLists.txt b/src/robot_description/CMakeLists.txt index 8f1cd2e..e84453f 100644 --- a/src/robot_description/CMakeLists.txt +++ b/src/robot_description/CMakeLists.txt @@ -21,5 +21,11 @@ install( ${CMAKE_CURRENT_SOURCE_DIR}/urdf/r5a_v_ros.urdf.xacro DESTINATION share/${PROJECT_NAME}/urdf ) +# Installer les matĂ©riaux +install(DIRECTORY media/materials/ + DESTINATION share/${PROJECT_NAME}/materials/) -ament_package() +install(DIRECTORY worlds/ + DESTINATION share/${PROJECT_NAME}/worlds/) + +ament_package() \ No newline at end of file diff --git a/src/robot_description/launch/gazebo.launch.py b/src/robot_description/launch/gazebo.launch.py index f5ac216..ebfcba6 100644 --- a/src/robot_description/launch/gazebo.launch.py +++ b/src/robot_description/launch/gazebo.launch.py @@ -1,9 +1,9 @@ """ @file gazebo.launch.py -@brief Launch file for setting up the robot simulation in Gazebo. +@brief Launch file for setting up the robot simulation in Gazebo with custom materials and lighting. -This launch file initializes the robot simulation in Gazebo, spawns the robot entity, -and starts the necessary nodes and controllers for the robot operation. +This launch file initializes the robot simulation in Gazebo with custom spotlight world, +custom materials, spawns the robot entity, and starts the necessary nodes and controllers. """ import os @@ -17,6 +17,7 @@ ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, + SetEnvironmentVariable, ) from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -26,52 +27,96 @@ def generate_launch_description(): """ @brief Generates the launch description for the robot simulation. - This function sets up the robot description, launches Gazebo, spawns the robot entity, - and starts the necessary nodes and controllers. + This function sets up the robot description, launches Gazebo with custom spotlight world, + loads custom materials, spawns the robot entity, and starts the necessary controllers. @return LaunchDescription object containing all the nodes and configurations to launch. """ - # Specify the name of the package and path to xacro file within the package - pkg_name = "robot_description" # Name of the robot description package - share_dir = get_package_share_directory( - pkg_name - ) # Get the share directory of the package - - # Use xacro to process the file - xacro_file = os.path.join( - share_dir, "urdf", "r5a_v_ros.urdf.xacro" - ) # Full path to the XACRO file - robot_description_xacro = xacro.process_file(xacro_file) # Process the XACRO file - robot_urdf = ( - robot_description_xacro.toxml() - ) # Convert the processed XACRO to URDF XML - - # Configure the robot_state_publisher node + # Package configuration + pkg_name = "robot_description" + share_dir = get_package_share_directory(pkg_name) + + print("🚀 Initializing Robot Simulation with Custom Materials and Lighting") + print("=" * 70) + + # Configure paths for custom materials + materials_path = os.path.join(share_dir, "materials") + world_file = os.path.join(share_dir, "worlds", "spotlight.world") + xacro_file = os.path.join(share_dir, "urdf", "r5a_v_ros.urdf.xacro") + + # Check materials + if os.path.exists(materials_path): + print(f"✅ Custom materials found: {materials_path}") + if os.path.exists( + os.path.join(materials_path, "scripts", "robot_materials_PLA.material") + ): + print("✅ PLA materials file found") + else: + print("⚠ robot_materials_PLA.material not found in scripts/") + else: + print(f"⚠ Materials directory not found: {materials_path}") + print(" Robot will use default Gazebo materials") + + # Check world file + if os.path.exists(world_file): + print(f"✅ Custom spotlight world found: {world_file}") + use_custom_world = True + else: + print(f"⚠ Spotlight world not found: {world_file}") + print(" Using default Gazebo world") + use_custom_world = False + + # Check URDF file + if os.path.exists(xacro_file): + print(f"✅ Robot URDF found: {xacro_file}") + else: + print(f"❌ Robot URDF not found: {xacro_file}") + + print("=" * 70) + + # Process robot description + robot_description_xacro = xacro.process_file(xacro_file) + robot_urdf = robot_description_xacro.toxml() + + # Environment variables for custom materials + set_gazebo_resource_path = SetEnvironmentVariable( + name="GAZEBO_RESOURCE_PATH", + value=[materials_path, ":", os.environ.get("GAZEBO_RESOURCE_PATH", "")], + ) + + set_gazebo_model_path = SetEnvironmentVariable( + name="GAZEBO_MODEL_PATH", + value=[share_dir, ":", os.environ.get("GAZEBO_MODEL_PATH", "")], + ) + + # Robot state publisher node node_robot_state_publisher = Node( - package="robot_state_publisher", # Package containing the node - executable="robot_state_publisher", # Executable name - output="screen", # Output mode + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", parameters=[ {"robot_description": robot_urdf}, {"use_sim_time": True}, - ], # Parameters + ], ) - # Node to spawn the entity in Gazebo - spawn_entity = Node( - package="gazebo_ros", # Package containing the node - executable="spawn_entity.py", # Executable script to spawn entities - arguments=[ - "-topic", - "/robot_description", - "-entity", - "armr5", - ], # Arguments for spawning - output="screen", - ) - - # Include the Gazebo launch file + # Gazebo launch configuration + gazebo_launch_args = { + "verbose": "true", + "pause": "false", + "use_sim_time": "true", + } + + # Add custom world if available + if use_custom_world: + gazebo_launch_args["world"] = world_file + print( + "🌟 Gazebo will launch with custom spotlight world for enhanced reflections" + ) + + # Gazebo server and client launch gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -79,10 +124,31 @@ def generate_launch_description(): "/gazebo.launch.py", ] ), + launch_arguments=gazebo_launch_args.items(), ) - # Commands to load and start controllers after spawning the robot - load_joint_states_controller = ExecuteProcess( + # Robot spawn node + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + name="spawn_entity", + arguments=[ + "-topic", + "/robot_description", + "-entity", + "armr5", + "-x", + "0.0", + "-y", + "0.0", + "-z", + "0.1", + ], + output="screen", + ) + + # Joint state broadcaster controller + load_joint_state_broadcaster = ExecuteProcess( cmd=[ "ros2", "control", @@ -90,10 +156,11 @@ def generate_launch_description(): "--set-state", "active", "joint_state_broadcaster", - ], # Command to load and activate joint_state_broadcaster + ], output="screen", ) + # Arm controller load_arm_controller = ExecuteProcess( cmd=[ "ros2", @@ -102,27 +169,70 @@ def generate_launch_description(): "--set-state", "active", "arm_controller", - ], # Command to load and activate arm_controller + ], + output="screen", + ) + + # Joint state publisher (for manual control if needed) + joint_state_publisher = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + parameters=[{"use_sim_time": True}], + condition=lambda: not os.path.exists("/tmp/gazebo_controllers_active"), + ) + + # Startup info node + info_node = ExecuteProcess( + cmd=[ + "bash", + "-c", + "sleep 5 && echo '✅ Robot simulation ready!' && " + "echo '🎼 Test robot movement:' && " + "echo 'ros2 topic pub /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory \"{joint_names: [R0_Yaw, R1_Pitch, R2_Pitch, R3_Yaw, R4_Pitch], points: [{positions: [0.5, 0.5, 0.5, 0.5, 0.5], time_from_start: {sec: 3}}]}\" --once'", + ], output="screen", ) - # Return the LaunchDescription with all the nodes and event handlers + # Launch description with proper sequencing return LaunchDescription( [ + # Environment setup + set_gazebo_resource_path, + set_gazebo_model_path, + # Core simulation gazebo, node_robot_state_publisher, + joint_state_publisher, + # Sequential robot spawning and controller loading + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gazebo, + on_exit=[ + ExecuteProcess( + cmd=["sleep", "3"], output="screen" + ), # Wait for Gazebo + ], + ) + ), spawn_entity, RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, - on_exit=[load_joint_states_controller], + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_states_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_arm_controller], ) ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_arm_controller, + on_exit=[info_node], + ) + ), ] ) diff --git a/src/robot_description/launch/gazebo_move.launch.py b/src/robot_description/launch/gazebo_move.launch.py index 9b8d312..043dd4b 100644 --- a/src/robot_description/launch/gazebo_move.launch.py +++ b/src/robot_description/launch/gazebo_move.launch.py @@ -2,9 +2,9 @@ @file gazebo_move.launch.py @brief Launch file for setting up the robot simulation and MoveIt configurations. -This launch file initializes the robot simulation in Gazebo, spawns the robot entity, -sets up the MoveIt configuration, and starts the necessary nodes and controllers -for the robot operation. +This launch file initializes the robot simulation in Gazebo with custom spotlight world, +spawns the robot entity, sets up the MoveIt configuration, and starts the necessary +nodes and controllers for the robot operation. Modified to include custom plastic black materials. """ import os @@ -19,6 +19,7 @@ ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, + SetEnvironmentVariable, ) from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -27,8 +28,8 @@ def generate_launch_description(): """ @brief Generates the launch description for the robot simulation and MoveIt setup. - This function sets up the robot description, launches Gazebo, spawns the robot entity, - configures MoveIt, and starts the necessary nodes and controllers. + This function sets up the robot description, launches Gazebo with custom spotlight world, + spawns the robot entity, configures MoveIt, and starts the necessary nodes and controllers. @return LaunchDescription object containing all the nodes and configurations to launch. """ @@ -39,6 +40,30 @@ def generate_launch_description(): pkg_name ) # Get the share directory of the package + # Configure custom materials path for Gazebo + materials_path = os.path.join(share_dir, "materials") + + # Path to custom spotlight world + world_file = os.path.join(share_dir, "worlds", "spotlight.world") + + # Check if materials directory exists + if not os.path.exists(materials_path): + print(f"⚠ Warning: Materials directory not found at {materials_path}") + print( + " Robot will use default Gazebo materials instead of custom plastic materials" + ) + else: + print(f"✅ Custom materials found at: {materials_path}") + + # Check if custom world exists + if os.path.exists(world_file): + print(f"✅ Using custom spotlight world: {world_file}") + print(f"🌟 Enhanced lighting will be used for better reflections") + else: + print(f"⚠ Custom spotlight world not found at {world_file}") + print(" Using default Gazebo world instead") + world_file = "" # Use default world + # Use xacro to process the file xacro_file = os.path.join( share_dir, "urdf", "r5a_v_ros.urdf.xacro" @@ -72,7 +97,17 @@ def generate_launch_description(): output="screen", ) - # Include the Gazebo launch file + # Include the Gazebo launch file with custom world and materials + gazebo_launch_args = { + "verbose": "true", # Enable verbose output for debugging materials + "pause": "false", # Don't pause simulation on start + } + + # Add world file if it exists + if world_file: + gazebo_launch_args["world"] = world_file + print(f"💡 Gazebo will launch with spotlight world for maximum reflections") + gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -80,6 +115,7 @@ def generate_launch_description(): "/gazebo.launch.py", ] ), + launch_arguments=gazebo_launch_args.items(), ) # Commands to load and start controllers after spawning the robot @@ -110,9 +146,7 @@ def generate_launch_description(): # MoveIt configuration using MoveItConfigsBuilder moveit_config = ( MoveItConfigsBuilder("robot_moveit_config", package_name="robot_moveit_config") - .robot_description( - file_path=xacro_file, mappings={"use_sim_time": "true"} - ) + .robot_description(file_path=xacro_file, mappings={"use_sim_time": "true"}) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") .joint_limits("config/joint_limits.yaml") @@ -135,12 +169,39 @@ def generate_launch_description(): parameters=[config_dict], # Parameters including MoveIt configurations ) + # Environment variables to configure Gazebo for custom materials + set_gazebo_resource_path = SetEnvironmentVariable( + name="GAZEBO_RESOURCE_PATH", + value=[materials_path, ":", os.environ.get("GAZEBO_RESOURCE_PATH", "")], + ) + + set_gazebo_model_path = SetEnvironmentVariable( + name="GAZEBO_MODEL_PATH", + value=[share_dir, ":", os.environ.get("GAZEBO_MODEL_PATH", "")], + ) + + # Optional: Add a small delay before spawning to ensure Gazebo is fully loaded + delay_spawn_entity = ExecuteProcess(cmd=["sleep", "2"], output="screen") + # Return the LaunchDescription with all the nodes and event handlers return LaunchDescription( [ + # Set environment variables first for custom materials + set_gazebo_resource_path, + set_gazebo_model_path, + # Launch Gazebo with custom spotlight world gazebo, + # Robot state publisher node_robot_state_publisher, - spawn_entity, + # Delay before spawning to ensure Gazebo is ready + delay_spawn_entity, + # Sequential controller and node loading with proper dependencies + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=delay_spawn_entity, + on_exit=[spawn_entity], + ) + ), RegisterEventHandler( event_handler=OnProcessExit( target_action=spawn_entity, diff --git a/src/robot_description/media/materials/scripts/aruco_plane.material b/src/robot_description/media/materials/scripts/aruco_plane.material deleted file mode 100644 index dbe8e0d..0000000 --- a/src/robot_description/media/materials/scripts/aruco_plane.material +++ /dev/null @@ -1,239 +0,0 @@ -material id0_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id0.png - scale 1 1 - } - } - } -} - -material id1_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id1.png - scale 1 1 - } - } - } -} - -material id2_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id2.png - scale 1 1 - } - } - } -} - -material id3_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id3.png - scale 1 1 - } - } - } -} - -material id4_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id4.png - scale 1 1 - } - } - } -} - -material id5_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id5.png - scale 1 1 - } - } - } -} - -material id6_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id6.png - scale 1 1 - } - } - } -} - -material id7_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id7.png - scale 1 1 - } - } - } -} - -material id8_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id8.png - scale 1 1 - } - } - } -} - -material id9_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id9.png - scale 1 1 - } - } - } -} - -material id10_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id10.png - scale 1 1 - } - } - } -} - -material id11_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id11.png - scale 1 1 - } - } - } -} - -material id12_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id12.png - scale 1 1 - } - } - } -} - -material id13_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id13.png - scale 1 1 - } - } - } -} - -material id14_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id14.png - scale 1 1 - } - } - } -} - -material id15_Plane_Material -{ - technique - { - pass - { - texture_unit - { - texture id15.png - scale 1 1 - } - } - } -} diff --git a/src/robot_description/media/materials/scripts/robot_materials_PLA.material b/src/robot_description/media/materials/scripts/robot_materials_PLA.material new file mode 100644 index 0000000..70a5c1a --- /dev/null +++ b/src/robot_description/media/materials/scripts/robot_materials_PLA.material @@ -0,0 +1,27 @@ +material PlasticBlackGlossy +{ + technique + { + pass + { + ambient 0.1 0.1 0.1 1.0 + diffuse 0.15 0.15 0.15 1.0 + specular 0.3 0.3 0.3 1.0 50.0 + emissive 0.0 0.0 0.0 1.0 + } + } +} +material PlasticOffWhite +{ + technique + { + pass + { + ambient 0.75 0.75 0.7 1.0 + diffuse 0.9 0.9 0.85 1.0 + specular 0.2 0.2 0.2 1.0 30.0 + emissive 0.0 0.0 0.0 1.0 + + } + } +} \ No newline at end of file diff --git a/src/robot_description/media/materials/textures/id0.png b/src/robot_description/media/materials/textures/id0.png deleted file mode 100644 index 7794fe5..0000000 Binary files a/src/robot_description/media/materials/textures/id0.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id1.png b/src/robot_description/media/materials/textures/id1.png deleted file mode 100644 index 29cac33..0000000 Binary files a/src/robot_description/media/materials/textures/id1.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id10.png b/src/robot_description/media/materials/textures/id10.png deleted file mode 100644 index 776b1d4..0000000 Binary files a/src/robot_description/media/materials/textures/id10.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id11.png b/src/robot_description/media/materials/textures/id11.png deleted file mode 100644 index e74af18..0000000 Binary files a/src/robot_description/media/materials/textures/id11.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id12.png b/src/robot_description/media/materials/textures/id12.png deleted file mode 100644 index 1f419da..0000000 Binary files a/src/robot_description/media/materials/textures/id12.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id13.png b/src/robot_description/media/materials/textures/id13.png deleted file mode 100644 index 81a960c..0000000 Binary files a/src/robot_description/media/materials/textures/id13.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id14.png b/src/robot_description/media/materials/textures/id14.png deleted file mode 100644 index cb9b301..0000000 Binary files a/src/robot_description/media/materials/textures/id14.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id15.png b/src/robot_description/media/materials/textures/id15.png deleted file mode 100644 index 8567ab1..0000000 Binary files a/src/robot_description/media/materials/textures/id15.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id16.png b/src/robot_description/media/materials/textures/id16.png deleted file mode 100644 index e1bdb01..0000000 Binary files a/src/robot_description/media/materials/textures/id16.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id17.png b/src/robot_description/media/materials/textures/id17.png deleted file mode 100644 index 58dab62..0000000 Binary files a/src/robot_description/media/materials/textures/id17.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id18.png b/src/robot_description/media/materials/textures/id18.png deleted file mode 100644 index 51029d7..0000000 Binary files a/src/robot_description/media/materials/textures/id18.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id19.png b/src/robot_description/media/materials/textures/id19.png deleted file mode 100644 index 66f00b0..0000000 Binary files a/src/robot_description/media/materials/textures/id19.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id2.png b/src/robot_description/media/materials/textures/id2.png deleted file mode 100644 index ebb97c9..0000000 Binary files a/src/robot_description/media/materials/textures/id2.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id3.png b/src/robot_description/media/materials/textures/id3.png deleted file mode 100644 index f2a82c0..0000000 Binary files a/src/robot_description/media/materials/textures/id3.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id4.png b/src/robot_description/media/materials/textures/id4.png deleted file mode 100644 index 85c7143..0000000 Binary files a/src/robot_description/media/materials/textures/id4.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id5.png b/src/robot_description/media/materials/textures/id5.png deleted file mode 100644 index 4d8f1c2..0000000 Binary files a/src/robot_description/media/materials/textures/id5.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id6.png b/src/robot_description/media/materials/textures/id6.png deleted file mode 100644 index a4a573b..0000000 Binary files a/src/robot_description/media/materials/textures/id6.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id7.png b/src/robot_description/media/materials/textures/id7.png deleted file mode 100644 index 1b10bea..0000000 Binary files a/src/robot_description/media/materials/textures/id7.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id8.png b/src/robot_description/media/materials/textures/id8.png deleted file mode 100644 index 2479527..0000000 Binary files a/src/robot_description/media/materials/textures/id8.png and /dev/null differ diff --git a/src/robot_description/media/materials/textures/id9.png b/src/robot_description/media/materials/textures/id9.png deleted file mode 100644 index 3e42111..0000000 Binary files a/src/robot_description/media/materials/textures/id9.png and /dev/null differ diff --git a/src/robot_description/models/aruco_block_long/meshes/Block_Long.png b/src/robot_description/models/aruco_block_long/meshes/Block_Long.png deleted file mode 100755 index afbd322..0000000 Binary files a/src/robot_description/models/aruco_block_long/meshes/Block_Long.png and /dev/null differ diff --git a/src/robot_description/models/aruco_block_long/meshes/block_long.dae b/src/robot_description/models/aruco_block_long/meshes/block_long.dae deleted file mode 100755 index e0f3fad..0000000 --- a/src/robot_description/models/aruco_block_long/meshes/block_long.dae +++ /dev/null @@ -1,144 +0,0 @@ - - - - - Blender User - Blender 2.80.75 commit date:2019-07-29, commit time:14:47, hash:f6cb5f54494e - - 2022-01-26T22:08:27 - 2022-01-26T22:08:27 - - Z_UP - - - - - - - Block_Long_png - - - - - Block_Long_png-surface - - - - - - 0 0 0 1 - - - - - - 1.45 - - - - - - - - - - - 0 0 0 1 - - - 0.8 0.8 0.8 1 - - - 1.45 - - - - - - - - - Block_Long.png - - - - - - - - - - - - - - - -55 -18 25 -55 -18 -25 55 -18 25 55 -18 -25 -55 -35 25 55 -35 25 -55 -35 -25 55 -35 -25 -55 18 -25 -55 18 25 55 18 -25 55 18 25 -55 35 -25 55 35 -25 -55 35 25 55 35 25 55 -35 35 -55 -35 35 -55 -35 -35 -55 35 -35 -55 35 35 55 35 35 55 -35 -35 55 35 -35 - - - - - - - - - - 0 -1 0 0 0 -1 0 0 1 0 1 0 -1 0 0 1 0 0 - - - - - - - - - - 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 7.97597e-5 0.9999202 7.97597e-5 7.97597e-5 0.636342 0.9999202 0.636342 0.9999202 7.97597e-5 7.97597e-5 0.636342 7.97944e-5 0 0 1 0 1 1 0 0 1 0 1 1 - - - - - - - - - - - - - - -

21 2 120 20 2 121 16 2 122 16 2 123 20 2 124 17 2 125

- - - - - -

0 0 0 1 0 1 2 0 2 2 0 3 1 0 4 3 0 5 4 1 6 0 1 7 5 1 8 5 1 9 0 1 10 2 1 11 1 2 12 6 2 13 3 2 14 3 2 15 6 2 16 7 2 17 8 3 18 9 3 19 10 3 20 10 3 21 9 3 22 11 3 23 12 2 24 8 2 25 13 2 26 13 2 27 8 2 28 10 2 29 9 1 30 14 1 31 11 1 32 11 1 33 14 1 34 15 1 35 5 0 36 16 0 37 4 0 38 4 0 39 16 0 40 17 0 41 6 4 42 1 4 43 18 4 44 18 4 45 1 4 46 8 4 47 18 4 48 8 4 49 19 4 50 19 4 51 8 4 52 12 4 53 1 4 54 0 4 55 8 4 56 8 4 57 0 4 58 9 4 59 9 4 60 0 4 61 17 4 62 9 4 63 17 4 64 20 4 65 0 4 66 4 4 67 17 4 68 20 4 69 14 4 70 9 4 71 5 5 72 2 5 73 16 5 74 16 5 75 2 5 76 11 5 77 16 5 78 11 5 79 21 5 80 21 5 81 11 5 82 15 5 83 2 5 84 3 5 85 11 5 86 11 5 87 3 5 88 10 5 89 10 5 90 3 5 91 22 5 92 10 5 93 22 5 94 23 5 95 3 5 96 7 5 97 22 5 98 23 5 99 13 5 100 10 5 101 13 3 102 23 3 103 12 3 104 12 3 105 23 3 106 19 3 107 6 0 108 18 0 109 7 0 110 7 0 111 18 0 112 22 0 113 19 1 114 23 1 115 18 1 116 18 1 117 23 1 118 22 1 119 14 3 126 20 3 127 15 3 128 15 3 129 20 3 130 21 3 131

- - - - - - - - 0.001 0 0 0 0 0.001 0 0 0 0 0.001 0 0 0 0 1 - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/robot_description/models/aruco_block_long/meshes/block_long.stl b/src/robot_description/models/aruco_block_long/meshes/block_long.stl deleted file mode 100755 index a9f6282..0000000 Binary files a/src/robot_description/models/aruco_block_long/meshes/block_long.stl and /dev/null differ diff --git a/src/robot_description/models/aruco_block_long/model.config b/src/robot_description/models/aruco_block_long/model.config deleted file mode 100755 index d405451..0000000 --- a/src/robot_description/models/aruco_block_long/model.config +++ /dev/null @@ -1,16 +0,0 @@ - - - - aruco_block_long - 1.0 - model.sdf - - - My - omar.sossi-alaoui@rtone.fr - - - - A block. - - diff --git a/src/robot_description/models/aruco_block_long/model.sdf b/src/robot_description/models/aruco_block_long/model.sdf deleted file mode 100755 index e8dea97..0000000 --- a/src/robot_description/models/aruco_block_long/model.sdf +++ /dev/null @@ -1,43 +0,0 @@ - - - - false - - - 0.349 - - 1.45e-4 - 0.0 - 0.0 - 1.45e-4 - 0.0 - 1.45e-4 - - - - - - 1 1 1 - model://aruco_block_long - - - - - - 1.2 - 1.2 - - - - - - - - 1 1 1 - model://aruco_block_long - - - - - - diff --git a/src/robot_description/models/checkerboard/checkerboard_plane.sdf b/src/robot_description/models/checkerboard/checkerboard_plane.sdf deleted file mode 100644 index abdd3c5..0000000 --- a/src/robot_description/models/checkerboard/checkerboard_plane.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - true - - - - - 0.3 0.3 0.3 - model://checkerboard_plane - - - - - - \ No newline at end of file diff --git a/src/robot_description/models/checkerboard/materials/textures/checker.png b/src/robot_description/models/checkerboard/materials/textures/checker.png deleted file mode 100644 index 2e37021..0000000 Binary files a/src/robot_description/models/checkerboard/materials/textures/checker.png and /dev/null differ diff --git a/src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae b/src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae deleted file mode 100644 index e85746a..0000000 --- a/src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae +++ /dev/null @@ -1,126 +0,0 @@ - - - - - Blender User - Blender 2.78.0 - - 2017-04-04T15:56:25 - 2017-04-04T15:56:25 - - Z_UP - - - - checker.png - - - - - - - - checker_png - - - - - checker_png-surface - - - - - - 1 1 1 1 - - - 1 1 1 1 - - - - - - 0 0 0 1 - - - 50 - - - 1 - - - - - - - - - - - - - - - - -1 -1 0 1 -1 0 -1 1 0 1 1 0 - - - - - - - - - - 0 0 1 - - - - - - - - - - 0.9999001 9.998e-5 9.998e-5 0.9999001 1.0004e-4 9.998e-5 0.9999001 9.998e-5 0.9999001 0.9999001 9.998e-5 0.9999001 - - - - - - - - - - - - - - - 3 3 -

1 0 0 2 0 1 0 0 2 1 0 3 3 0 4 2 0 5

- - - - - - - - - 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/robot_description/models/checkerboard/model.config b/src/robot_description/models/checkerboard/model.config deleted file mode 100644 index 58b0f1d..0000000 --- a/src/robot_description/models/checkerboard/model.config +++ /dev/null @@ -1,16 +0,0 @@ - - - - Checkerboard Plane - 1.0 - checkerboard_plane.sdf - - - Adam Allevato - adam.d.allevato@gmail.com - - - - A plane with a reference checkerboard texture on it, used to validate and calibrate camera models. The checkerboard is 8x8 squares (like a standard checkerboard), and measures 2m on each side. - - diff --git a/src/robot_description/urdf/aruco.urdf.xacro b/src/robot_description/urdf/aruco.urdf.xacro deleted file mode 100644 index af3e1ce..0000000 --- a/src/robot_description/urdf/aruco.urdf.xacro +++ /dev/null @@ -1,84 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - id${plane_number}_Plane_Material - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/robot_description/urdf/r5a_v_ros.urdf.xacro b/src/robot_description/urdf/r5a_v_ros.urdf.xacro index 0512b25..512f1de 100644 --- a/src/robot_description/urdf/r5a_v_ros.urdf.xacro +++ b/src/robot_description/urdf/r5a_v_ros.urdf.xacro @@ -1,6 +1,7 @@ - + + @@ -686,52 +687,82 @@ effort="5" velocity="1" /> - - Gazebo/White + + + + + PlasticBlackGlossy + 0.8 + 0.8 + 1000000.0 + 1.0 - Gazebo/White + PlasticBlackGlossy + 0.8 + 0.8 - Gazebo/White + PlasticBlackGlossy + 0.8 + 0.8 - Gazebo/White + PlasticBlackGlossy + 0.8 + 0.8 - Gazebo/White + PlasticBlackGlossy + 0.8 + 0.8 - Gazebo/Black + PlasticBlackGlossy + 0.8 + 0.8 + - Gazebo/White + PlasticBlackGlossy + 0.9 + 0.9 - Gazebo/Black + PlasticBlackGlossy + 0.9 + 0.9 - Gazebo/White + PlasticOffWhite + 0.9 + 0.9 - Gazebo/White + PlasticBlackGlossy + 0.9 + 0.9 - Gazebo/Black + PlasticBlackGlossy + 0.9 + 0.9 - Gazebo/White + PlasticOffWhite + 0.9 + 0.9 @@ -990,4 +1021,4 @@ - + \ No newline at end of file diff --git a/src/robot_description/worlds/spotlight.world b/src/robot_description/worlds/spotlight.world index 4d80085..849bd90 100644 --- a/src/robot_description/worlds/spotlight.world +++ b/src/robot_description/worlds/spotlight.world @@ -1,54 +1,150 @@ + + + + 0.6 0.6 0.6 1.0 + 0.7 0.7 0.7 1.0 + 1 + + model://ground_plane - - - model://sun - - - - - 1 0 0.55 0 1.45 0 - 1.0 1.0 1.0 1.0 - 0.1 0.1 0.1 1.0 + + + 1 + 0 0 10 0 0 0 + 1.5 1.5 1.5 1 + 2.0 2.0 2.0 1 - 10.0 - 0.3 + 1000 + 0.9 0.01 0.001 + -0.5 0.1 -0.9 + + + + + 1 0 0.55 0 1.45 0 + 1.2 1.2 1.2 1.0 + 1.5 1.5 1.5 1.0 + + 15.0 + 0.1 + 0.005 + 0.0005 + 0 0 -1 - 0.5 - 1.0 + 0.4 + 0.8 1.0 true - + - -0.7 0.7 0.85 0 1.1854 2.35 - 1.0 1.0 1.0 1.0 - 0.1 0.1 0.1 1.0 + -0.7 0.7 0.85 0 1.1854 2.35 + 1.2 1.2 1.2 1.0 + 1.5 1.5 1.5 1.0 - 10.0 - 0.3 + 15.0 + 0.1 + 0.005 + 0.0005 + + 0 0 -1 + + 0.4 + 0.8 + 1.0 + + true + + + + + 0 1.5 1.2 0 0.8 -1.57 + 0.8 0.8 0.8 1.0 + 1.2 1.2 1.2 1.0 + + 12.0 + 0.15 0.01 0.001 0 0 -1 - 0.5 + 0.6 1.0 1.0 - true + false + + + + + 0 0 3 0 0 0 + 0.8 0.8 0.8 1 + 1.0 1.0 1.0 1 + + 8 + 0.2 + 0.05 + 0.005 + + false + + + + 2 -1 1.5 0 0 0 + 0.6 0.6 0.6 1 + 0.8 0.8 0.8 1 + + 6 + 0.3 + 0.08 + 0.01 + + false + + + + + 0 0 -9.8066 + + + quick + 150 + 1.3 + + + 0.00001 + 0.2 + 1000.0 + 0.01 + + + 0.004 + 1 + 250 + + + + + + /gazebo + + 1.0 + + - + \ No newline at end of file diff --git a/src/robot_moveit_config/config/armr5.srdf.backup_complete_20250721_165728 b/src/robot_moveit_config/config/armr5.srdf.backup_complete_20250721_165728 new file mode 100644 index 0000000..f6f4b84 --- /dev/null +++ b/src/robot_moveit_config/config/armr5.srdf.backup_complete_20250721_165728 @@ -0,0 +1,451 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_moveit_config/launch/demo.launch.py b/src/robot_moveit_config/launch/demo.launch.py index 32eac51..560da37 100644 --- a/src/robot_moveit_config/launch/demo.launch.py +++ b/src/robot_moveit_config/launch/demo.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_demo_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/move_group.launch.py b/src/robot_moveit_config/launch/move_group.launch.py index 5b4d32c..c60cdbd 100644 --- a/src/robot_moveit_config/launch/move_group.launch.py +++ b/src/robot_moveit_config/launch/move_group.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_move_group_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/moveit_rviz.launch.py b/src/robot_moveit_config/launch/moveit_rviz.launch.py index 1e1e999..83527f1 100644 --- a/src/robot_moveit_config/launch/moveit_rviz.launch.py +++ b/src/robot_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/rsp.launch.py b/src/robot_moveit_config/launch/rsp.launch.py index 7cc68d3..5cb54a3 100644 --- a/src/robot_moveit_config/launch/rsp.launch.py +++ b/src/robot_moveit_config/launch/rsp.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_rsp_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/setup_assistant.launch.py b/src/robot_moveit_config/launch/setup_assistant.launch.py index 38344a6..9c7992e 100644 --- a/src/robot_moveit_config/launch/setup_assistant.launch.py +++ b/src/robot_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/spawn_controllers.launch.py b/src/robot_moveit_config/launch/spawn_controllers.launch.py index 30685aa..74c4df2 100644 --- a/src/robot_moveit_config/launch/spawn_controllers.launch.py +++ b/src/robot_moveit_config/launch/spawn_controllers.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_spawn_controllers_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py b/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py index 7e47814..87c9684 100644 --- a/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ b/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/src/robot_moveit_config/launch/warehouse_db.launch.py b/src/robot_moveit_config/launch/warehouse_db.launch.py index 5abe91a..1ccb2d8 100644 --- a/src/robot_moveit_config/launch/warehouse_db.launch.py +++ b/src/robot_moveit_config/launch/warehouse_db.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("armr5", package_name="robot_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "armr5", package_name="robot_moveit_config" + ).to_moveit_configs() return generate_warehouse_db_launch(moveit_config) diff --git a/src/robot_test/data_analysis/scripts/generate_report.py b/src/robot_test/data_analysis/scripts/generate_report.py index cc27a49..adb86d6 100644 --- a/src/robot_test/data_analysis/scripts/generate_report.py +++ b/src/robot_test/data_analysis/scripts/generate_report.py @@ -6,6 +6,7 @@ # Set the logs directory LOGS_DIR = "logs" + def process_logs(): # Iterate through all folders in the logs directory for folder in os.listdir(LOGS_DIR): @@ -26,15 +27,23 @@ def process_logs(): detection_file = os.path.join(log_folder_path, "detected_markers.csv") info_file = os.path.join(log_folder_path, "test_information.txt") - if not os.path.exists(error_file) or not os.path.exists(detection_file) or not os.path.exists(info_file): + if ( + not os.path.exists(error_file) + or not os.path.exists(detection_file) + or not os.path.exists(info_file) + ): print(f"Missing required files in {folder}. Skipping...") continue # Analyze data and generate the report - analyze_and_generate_report(log_folder_path, error_file, detection_file, info_file, report_path) + analyze_and_generate_report( + log_folder_path, error_file, detection_file, info_file, report_path + ) -def analyze_and_generate_report(folder_path, error_file, detection_file, info_file, report_path): +def analyze_and_generate_report( + folder_path, error_file, detection_file, info_file, report_path +): # Load data errors_df = pd.read_csv(error_file).round(4) detections_df = pd.read_csv(detection_file) @@ -70,7 +79,9 @@ def analyze_and_generate_report(folder_path, error_file, detection_file, info_fi for marker_id in errors_df["marker_id"].unique(): marker_data = errors_df[errors_df["marker_id"] == marker_id] timestamps = marker_data["timestamp"] - errors = (marker_data["dx"]**2 + marker_data["dy"]**2 + marker_data["dz"]**2)**0.5 + errors = ( + marker_data["dx"] ** 2 + marker_data["dy"] ** 2 + marker_data["dz"] ** 2 + ) ** 0.5 plt.plot(timestamps, errors, label=f"Marker {marker_id}", marker="o") plt.title("Pose Errors Over Time") plt.xlabel("Time (s)") @@ -82,7 +93,13 @@ def analyze_and_generate_report(folder_path, error_file, detection_file, info_fi # Plot 2: Number of Detected Markers detection_plot_path = os.path.join(folder_path, "detection_plot.png") plt.figure(figsize=(10, 6)) - plt.plot(detections_df["timestamp"], detections_df["num_markers"], marker="o", color="blue", label="Detected Markers") + plt.plot( + detections_df["timestamp"], + detections_df["num_markers"], + marker="o", + color="blue", + label="Detected Markers", + ) plt.title("Number of Detected Markers Over Time") plt.xlabel("Time (s)") plt.ylabel("Number of Markers") @@ -97,7 +114,12 @@ def analyze_and_generate_report(folder_path, error_file, detection_file, info_fi plt.figure(figsize=(10, 6)) for marker_id in errors_df["marker_id"].unique(): marker_data = errors_df[errors_df["marker_id"] == marker_id] - plt.plot(marker_data["timestamp"], marker_data[axis], label=f"Marker {marker_id}", marker="o") + plt.plot( + marker_data["timestamp"], + marker_data[axis], + label=f"Marker {marker_id}", + marker="o", + ) plt.title(f"Orientation Errors ({axis.upper()}) Over Time") plt.xlabel("Time (s)") plt.ylabel(f"Orientation Error ({axis})") @@ -133,15 +155,28 @@ def analyze_and_generate_report(folder_path, error_file, detection_file, info_fi if overall_mean is not None: pdf.cell(200, 10, txt="Overall Pose Errors (Mean and Std):", ln=True) for col in overall_mean.index: - pdf.cell(200, 10, txt=f" {col}: Mean={overall_mean[col]:.4f}, Std={overall_std[col]:.4f}", ln=True) + pdf.cell( + 200, + 10, + txt=f" {col}: Mean={overall_mean[col]:.4f}, Std={overall_std[col]:.4f}", + ln=True, + ) pdf.cell(200, 10, txt="Detection Analysis:", ln=True, align="L") - pdf.cell(200, 10, txt=f"Mean Number of Detected Markers: {detection_mean:.4f}", ln=True) + pdf.cell( + 200, 10, txt=f"Mean Number of Detected Markers: {detection_mean:.4f}", ln=True + ) pdf.cell(200, 10, txt=f"Standard Deviation: {detection_std:.4f}", ln=True) # Page 4-5: Orientation Error Plots for axis, path in orientation_plot_paths.items(): pdf.add_page() - pdf.cell(200, 10, txt=f"Orientation Errors ({axis.upper()}) Over Time", ln=True, align="C") + pdf.cell( + 200, + 10, + txt=f"Orientation Errors ({axis.upper()}) Over Time", + ln=True, + align="C", + ) pdf.image(path, x=10, y=50, w=180) # Save the PDF diff --git a/src/robot_test/launch/standard_joint_test.launch.py b/src/robot_test/launch/standard_joint_test.launch.py index fc05816..5bc437a 100644 --- a/src/robot_test/launch/standard_joint_test.launch.py +++ b/src/robot_test/launch/standard_joint_test.launch.py @@ -27,6 +27,7 @@ import xacro from moveit_configs_utils import MoveItConfigsBuilder + def generate_launch_description(): """ @brief Generates the launch description for running standard joint tests with visual servoing components. @@ -42,56 +43,95 @@ def generate_launch_description(): # Declare the 'num_cameras' launch argument num_cameras_arg = DeclareLaunchArgument( - 'num_cameras', - default_value='1', - description='Number of cameras (1 or 2)' + "num_cameras", default_value="1", description="Number of cameras (1 or 2)" ) # Launch configuration to access 'num_cameras' argument - num_cameras = LaunchConfiguration('num_cameras') + num_cameras = LaunchConfiguration("num_cameras") # Package Directories pkg_name = "robot_description" # Name of the robot description package - robot_moveit_config = "robot_moveit_config" # Name of the MoveIt configuration package - share_dir = get_package_share_directory(pkg_name) # Path to the robot description package - moveit_config_pkg_path = get_package_share_directory(robot_moveit_config) # Path to the MoveIt config package + robot_moveit_config = ( + "robot_moveit_config" # Name of the MoveIt configuration package + ) + share_dir = get_package_share_directory( + pkg_name + ) # Path to the robot description package + moveit_config_pkg_path = get_package_share_directory( + robot_moveit_config + ) # Path to the MoveIt config package # Load and process URDF/XACRO file - xacro_file = os.path.join(share_dir, "urdf", "r5a_v_ros.urdf.xacro") # Path to the XACRO file + xacro_file = os.path.join( + share_dir, "urdf", "r5a_v_ros.urdf.xacro" + ) # Path to the XACRO file robot_description_config = xacro.process_file(xacro_file) # Process the XACRO file - robot_description = {"robot_description": robot_description_config.toxml()} # Convert to XML format + robot_description = { + "robot_description": robot_description_config.toxml() + } # Convert to XML format # Joint positions parameter (Define your joint positions here) joint_positions = [ # Position 0: Home position - 0.0, 0.0, 0.0, 0.0, 0.0, - + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, # Position 1: Extend first joint (Base rotation) - 0.5, 0.0, 0.0, 0.0, 0.0, - + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, # Position 2: Extend second joint (Shoulder) - 0.0, 0.5, 0.0, 0.0, 0.0, - + 0.0, + 0.5, + 0.0, + 0.0, + 0.0, # Position 3: Extend third joint (Elbow) - 0.0, 0.0, 0.5, 0.0, 0.0, - + 0.0, + 0.0, + 0.5, + 0.0, + 0.0, # Position 4: Extend fourth joint (Wrist pitch) - 0.0, 0.0, 0.0, 0.5, 0.0, - + 0.0, + 0.0, + 0.0, + 0.5, + 0.0, # Position 5: Extend fifth joint (Wrist roll) - 0.0, 0.0, 0.0, 0.0, 0.5, - + 0.0, + 0.0, + 0.0, + 0.0, + 0.5, # Position 6: All joints at positive quarter range - 0.25, 0.25, 0.25, 0.25, 0.25, - + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, # Position 7: All joints at negative quarter range - -0.25, -0.25, -0.25, -0.25, -0.25, - + -0.25, + -0.25, + -0.25, + -0.25, + -0.25, # Position 8: Alternate joints positive and negative - 0.5, -0.5, 0.5, -0.5, 0.5, - + 0.5, + -0.5, + 0.5, + -0.5, + 0.5, # Position 9: Arm Pointing towards Camera 1 - 1.5, -1.5, 0.0, 0.0, 0.0, + 1.5, + -1.5, + 0.0, + 0.0, + 0.0, ] # Test type parameter @@ -106,7 +146,9 @@ def generate_launch_description(): ) # Gazebo launch with a custom world file that includes a spotlight - world_file_path = os.path.join(share_dir, "worlds", "spotlight.world") # Path to the custom world file + world_file_path = os.path.join( + share_dir, "worlds", "spotlight.world" + ) # Path to the custom world file gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -214,7 +256,7 @@ def generate_launch_description(): parameters=[ {"use_sim_time": True}, ], - condition=UnlessCondition(PythonExpression(['"', num_cameras, '" == "2"'])) + condition=UnlessCondition(PythonExpression(['"', num_cameras, '" == "2"'])), ) # Aruco Detector Double Node @@ -225,14 +267,13 @@ def generate_launch_description(): parameters=[ {"use_sim_time": True}, ], - condition=IfCondition(PythonExpression(['"', num_cameras, '" == "2"'])) + condition=IfCondition(PythonExpression(['"', num_cameras, '" == "2"'])), ) # Launch Actions actions = [ num_cameras_arg, # Added the launch argument SetParameter(name="use_sim_time", value=True), - # Event handlers for controller loading RegisterEventHandler( event_handler=OnProcessExit( @@ -252,7 +293,6 @@ def generate_launch_description(): on_exit=[move_group_node], ) ), - # Event handler to start other nodes after move_group_node starts RegisterEventHandler( event_handler=OnProcessStart( @@ -265,17 +305,15 @@ def generate_launch_description(): ], ) ), - # Event handler to shutdown when standard_joint_test_node exits RegisterEventHandler( event_handler=OnProcessExit( target_action=standard_joint_test_node, on_exit=[ - Shutdown(reason='Standard joint test completed'), + Shutdown(reason="Standard joint test completed"), ], ) ), - # Nodes to be launched gazebo, robot_state_publisher_node,