diff --git a/src/robot_control/src/aruco_detector_single.cpp b/src/robot_control/src/aruco_detector_single.cpp deleted file mode 100644 index 7b693e1..0000000 --- a/src/robot_control/src/aruco_detector_single.cpp +++ /dev/null @@ -1,217 +0,0 @@ -/// @file aruco_detector_single.cpp -/// @brief Node that detects ArUco markers in images and broadcasts their poses as TF transforms. - -#include // Include ament_index_cpp -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/// @class ArucoDetectorSingle -/// @brief Detects ArUco markers in images and publishes their poses using TF. -class ArucoDetectorSingle : public rclcpp::Node { -public: - /// @brief Constructor for ArucoDetectorSingle. - ArucoDetectorSingle() : Node("aruco_detector_single"), tf_broadcaster_(this) { - // Get the package share directory - std::string package_share_directory = ament_index_cpp::get_package_share_directory("robot_control"); - - // Construct the full paths to the configuration files - std::string camera_calibration_file = package_share_directory + "/config/camera_calibration.yaml"; - std::string camera_transform_file = package_share_directory + "/config/transform.yaml"; - - // Read camera calibration parameters - readCameraCalibration(camera_calibration_file, camMatrix_, distCoeffs_); - readTransforms(camera_transform_file); - - // Set the marker length in meters - marker_length_ = 0.0425; - - // Create a subscription for receiving images - image_subscription_ = this->create_subscription( - "camera1/image_raw", 10, - std::bind(&ArucoDetectorSingle::imageCallback, this, std::placeholders::_1)); - } - -private: - /// @brief Reads camera calibration parameters from a file. - /// @param filename Path to the calibration file. - /// @param camMatrix Output camera matrix. - /// @param distCoeffs Output distortion coefficients. - void readCameraCalibration(const std::string &filename, cv::Mat &camMatrix, - cv::Mat &distCoeffs) { - cv::FileStorage fs(filename, cv::FileStorage::READ); - if (!fs.isOpened()) { - RCLCPP_ERROR(this->get_logger(), - "Failed to open camera calibration file: %s", - filename.c_str()); - return; - } - fs["camera_matrix"] >> camMatrix; - fs["distortion_coefficients"] >> distCoeffs; - fs.release(); - } - - /// @brief Reads transformation matrices from a YAML file. - /// @param filename Path to the YAML file. - void readTransforms(const std::string &filename) { - try { - YAML::Node config = YAML::LoadFile(filename); - if (config["camera"]) { - // Find the camera with id 1 - for (const auto &camera_node : config["camera"]) { - if (camera_node["id"].as() == 1) { - camera_transform_ = parseTransform(camera_node["transform"]); - break; - } - } - } else { - RCLCPP_ERROR(this->get_logger(), "No camera transforms found in the file: %s", - filename.c_str()); - } - } catch (const YAML::Exception &e) { - RCLCPP_ERROR(this->get_logger(), "YAML parsing error in '%s': %s", - filename.c_str(), e.what()); - } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), - "Error loading camera transforms from '%s': %s", - filename.c_str(), e.what()); - } - } - - /// @brief Parses a transformation matrix from a YAML node. - /// @param node YAML node containing the transform. - /// @return The transformation matrix as an Eigen::Matrix4d. - Eigen::Matrix4d parseTransform(const YAML::Node &node) { - Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); - for (int i = 0; i < node.size(); ++i) { - for (int j = 0; j < node[i].size(); ++j) { - transform(i, j) = node[i][j].as(); - } - } - return transform; - } - - /// @brief Callback function for processing received images. - /// @param msg The image message received. - void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { - cv::Mat frame; - try { - frame = cv_bridge::toCvShare(msg, "bgr8")->image; - } catch (cv_bridge::Exception &e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; - } - - // Detect ArUco markers - std::vector markerIds; - std::vector> markerCorners, rejectedCandidates; - cv::Ptr detectorParams = - cv::aruco::DetectorParameters::create(); - cv::Ptr dictionary = - cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); - cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds, - detectorParams, rejectedCandidates); - - // Refine corner locations to sub-pixel accuracy (optional) - if (!markerCorners.empty()) { - cv::Mat gray; - cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); - cv::TermCriteria criteria( - cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 100, 0.001); - for (auto &corners : markerCorners) { - cv::cornerSubPix(gray, corners, cv::Size(5, 5), cv::Size(-1, -1), - criteria); - } - } - - cv::Mat outputImage = frame.clone(); - if (!markerIds.empty()) { - size_t nMarkers = markerIds.size(); - std::vector rvecs, tvecs; - - // Estimate pose of markers - cv::aruco::estimatePoseSingleMarkers( - markerCorners, marker_length_, camMatrix_, distCoeffs_, rvecs, tvecs); - - for (size_t i = 0; i < nMarkers; i++) { - int marker_id = markerIds[i]; - - // Convert rotation vector to rotation matrix - cv::Mat rotation_matrix; - cv::Rodrigues(rvecs[i], rotation_matrix); - - // Draw axis for each marker - cv::drawFrameAxes(outputImage, camMatrix_, distCoeffs_, rvecs[i], - tvecs[i], marker_length_ * 1.5f); - - // Build the transformation matrix from camera to marker - Eigen::Matrix4d camera_to_marker = Eigen::Matrix4d::Identity(); - for (int row = 0; row < 3; ++row) { - for (int col = 0; col < 3; ++col) { - camera_to_marker(row, col) = rotation_matrix.at(row, col); - } - camera_to_marker(row, 3) = tvecs[i][row]; - } - - // Compute the transformation from the fixed frame to the ArUco marker - Eigen::Matrix4d fixed_to_marker = camera_transform_ * camera_to_marker; - - // Prepare the TransformStamped message - geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = this->now(); - transformStamped.header.frame_id = "world"; - transformStamped.child_frame_id = - "aruco_" + std::to_string(marker_id); - transformStamped.transform.translation.x = fixed_to_marker(0, 3); - transformStamped.transform.translation.y = fixed_to_marker(1, 3); - transformStamped.transform.translation.z = fixed_to_marker(2, 3); - - // Convert rotation matrix to quaternion - Eigen::Matrix3d rotation = fixed_to_marker.block<3, 3>(0, 0); - Eigen::Quaterniond quaternion(rotation); - transformStamped.transform.rotation.x = quaternion.x(); - transformStamped.transform.rotation.y = quaternion.y(); - transformStamped.transform.rotation.z = quaternion.z(); - transformStamped.transform.rotation.w = quaternion.w(); - - // Broadcast the transformation - tf_broadcaster_.sendTransform(transformStamped); - } - - // Draw detected markers on the image - cv::aruco::drawDetectedMarkers(outputImage, markerCorners, markerIds); - } - - // Display the image for verification (optional) - cv::imshow("Detected ArUco markers", outputImage); - cv::waitKey(1); - } - - rclcpp::Subscription::SharedPtr - image_subscription_; ///< Subscription to the image topic. - cv::Mat camMatrix_, distCoeffs_; ///< Camera calibration parameters. - tf2_ros::TransformBroadcaster - tf_broadcaster_; ///< TF broadcaster for publishing transforms. - Eigen::Matrix4d - camera_transform_; ///< Transformation from camera to fixed frame. - double marker_length_; ///< Marker length in meters. -}; - -/// @brief Main function that initializes and spins the ArucoDetectorSingle node. -/// @param argc Argument count. -/// @param argv Argument vector. -/// @return Exit status code. -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} 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_control/CMakeLists.txt b/src/src/robot_control/CMakeLists.txt similarity index 74% rename from src/robot_control/CMakeLists.txt rename to src/src/robot_control/CMakeLists.txt index dc8cd85..a3a0e1a 100644 --- a/src/robot_control/CMakeLists.txt +++ b/src/src/robot_control/CMakeLists.txt @@ -19,8 +19,6 @@ find_package(message_filters REQUIRED) find_package(tf2_eigen REQUIRED) find_package(ament_index_cpp REQUIRED) - - # Include directories include_directories( ${OpenCV_INCLUDE_DIRS} @@ -40,10 +38,13 @@ add_executable(moveit_control_simple src/moveit_control_simple.cpp) add_executable(end_pose_node src/end_pose_node.cpp) add_executable(aruco_detector_single src/aruco_detector_single.cpp) add_executable(moveit_control_gui src/moveit_control_gui.cpp) -add_executable(visual_joint_state_publisher src/visual_joint_state_publisher.cpp) add_executable(aruco_rotation_publisher src/aruco_rotation_publisher.cpp) add_executable(aruco_detector_double src/aruco_detector_double.cpp) +# REMOVED: visual_joint_state_publisher no longer needed with direct joint control +# add_executable(visual_joint_state_publisher src/visual_joint_state_publisher.cpp) + +# Link libraries and dependencies for aruco_detector_double ament_target_dependencies( aruco_detector_double rclcpp @@ -71,8 +72,7 @@ target_link_libraries(aruco_detector_double Eigen3::Eigen ) - -# Link libraries +# Link libraries and dependencies for aruco_rotation_publisher ament_target_dependencies(aruco_rotation_publisher rclcpp geometry_msgs @@ -81,13 +81,7 @@ ament_target_dependencies(aruco_rotation_publisher tf2_geometry_msgs ) -# Install the executable -install(TARGETS - aruco_rotation_publisher - DESTINATION lib/${PROJECT_NAME} -) - -# Link the required dependencies for moveit_control_gui +# Link libraries and dependencies for moveit_control_gui ament_target_dependencies(moveit_control_gui rclcpp moveit_ros_planning_interface @@ -95,12 +89,12 @@ ament_target_dependencies(moveit_control_gui tf2_ros ) -# Link Qt5Widgets using target_link_libraries +# Link Qt5Widgets for GUI target_link_libraries(moveit_control_gui Qt5::Widgets ) -# Link libraries for aruco_detector_single (ArucoDetectorSingle) +# Link libraries and dependencies for aruco_detector_single ament_target_dependencies( aruco_detector_single rclcpp @@ -123,14 +117,14 @@ target_link_libraries(aruco_detector_single Eigen3::Eigen ) -# Link libraries for moveit_control_simple +# Link libraries and dependencies for moveit_control_simple ament_target_dependencies(moveit_control_simple rclcpp moveit_ros_planning_interface geometry_msgs ) -# Link libraries for end_pose_node +# Link libraries and dependencies for end_pose_node ament_target_dependencies(end_pose_node rclcpp geometry_msgs @@ -138,28 +132,29 @@ ament_target_dependencies(end_pose_node tf2_geometry_msgs ) -# Link libraries for visual_joint_state_publisher -ament_target_dependencies(visual_joint_state_publisher - rclcpp - sensor_msgs - geometry_msgs - tf2_ros - tf2_geometry_msgs -) - -target_link_libraries(visual_joint_state_publisher - yaml-cpp - Eigen3::Eigen -) - -# Install targets +# REMOVED: Dependencies for visual_joint_state_publisher (no longer needed) +# ament_target_dependencies(visual_joint_state_publisher +# rclcpp +# sensor_msgs +# geometry_msgs +# tf2_ros +# tf2_geometry_msgs +# ) +# +# target_link_libraries(visual_joint_state_publisher +# yaml-cpp +# Eigen3::Eigen +# ) + +# Install executables (visual_joint_state_publisher removed) install(TARGETS aruco_detector_single moveit_control_simple end_pose_node moveit_control_gui - visual_joint_state_publisher + aruco_rotation_publisher aruco_detector_double + # REMOVED: visual_joint_state_publisher - no longer needed with direct joint angle control DESTINATION lib/${PROJECT_NAME} ) @@ -168,4 +163,4 @@ install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/ ) -ament_package() +ament_package() \ No newline at end of file diff --git a/src/robot_control/config/aruco_to_link.yaml b/src/src/robot_control/config/aruco_to_link.yaml similarity index 100% rename from src/robot_control/config/aruco_to_link.yaml rename to src/src/robot_control/config/aruco_to_link.yaml diff --git a/src/robot_control/config/camera_calibration.yaml b/src/src/robot_control/config/camera_calibration.yaml similarity index 100% rename from src/robot_control/config/camera_calibration.yaml rename to src/src/robot_control/config/camera_calibration.yaml diff --git a/src/robot_control/config/moveit_control.yaml b/src/src/robot_control/config/moveit_control.yaml similarity index 100% rename from src/robot_control/config/moveit_control.yaml rename to src/src/robot_control/config/moveit_control.yaml diff --git a/src/robot_control/config/transform.yaml b/src/src/robot_control/config/transform.yaml similarity index 100% rename from src/robot_control/config/transform.yaml rename to src/src/robot_control/config/transform.yaml diff --git a/src/robot_control/launch/gazebo_gui.launch.py b/src/src/robot_control/launch/gazebo_gui.launch.py similarity index 99% rename from src/robot_control/launch/gazebo_gui.launch.py rename to src/src/robot_control/launch/gazebo_gui.launch.py index 502cb57..d60ad89 100644 --- a/src/robot_control/launch/gazebo_gui.launch.py +++ b/src/src/robot_control/launch/gazebo_gui.launch.py @@ -146,6 +146,7 @@ def generate_launch_description(): {"use_sim_time": True}, {"moveit_current_state_monitor.joint_state_qos": "sensor_data"}, ], # Pass the MoveIt config to the GUI node + # arguments=['--ros-args', '--log-level', 'debug'], ) # Return the LaunchDescription diff --git a/src/robot_control/launch/moveit_control.launch.py b/src/src/robot_control/launch/moveit_control.launch.py similarity index 76% rename from src/robot_control/launch/moveit_control.launch.py rename to src/src/robot_control/launch/moveit_control.launch.py index a0cb24e..43b60bc 100644 --- a/src/robot_control/launch/moveit_control.launch.py +++ b/src/src/robot_control/launch/moveit_control.launch.py @@ -8,7 +8,6 @@ import os from launch import LaunchDescription -from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder @@ -23,22 +22,11 @@ def generate_launch_description(): @return LaunchDescription object containing the nodes 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 - - # MoveIt configuration using MoveItConfigsBuilder + # Load the 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"} + file_path="config/r5a_v_ros.urdf.xacro", mappings={"use_sim_time": "true"} ) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") diff --git a/src/robot_control/launch/moveit_control2.launch.py b/src/src/robot_control/launch/moveit_control2.launch.py similarity index 72% rename from src/robot_control/launch/moveit_control2.launch.py rename to src/src/robot_control/launch/moveit_control2.launch.py index f1adaa4..7775394 100644 --- a/src/robot_control/launch/moveit_control2.launch.py +++ b/src/src/robot_control/launch/moveit_control2.launch.py @@ -8,7 +8,6 @@ import os from launch import LaunchDescription -from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder @@ -22,22 +21,11 @@ def generate_launch_description(): @return LaunchDescription object containing the robot control node. """ - # 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 - - # MoveIt configuration using MoveItConfigsBuilder + # Load the 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"} + file_path="config/r5a_v_ros.urdf.xacro", mappings={"use_sim_time": "true"} ) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") diff --git a/src/robot_control/launch/moveit_control_gui.launch.py b/src/src/robot_control/launch/moveit_control_gui.launch.py similarity index 74% rename from src/robot_control/launch/moveit_control_gui.launch.py rename to src/src/robot_control/launch/moveit_control_gui.launch.py index 55c4fc1..a129112 100644 --- a/src/robot_control/launch/moveit_control_gui.launch.py +++ b/src/src/robot_control/launch/moveit_control_gui.launch.py @@ -8,7 +8,6 @@ import os from launch import LaunchDescription -from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder @@ -23,22 +22,11 @@ def generate_launch_description(): @return LaunchDescription object containing the GUI node. """ - # 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 - - # MoveIt configuration using MoveItConfigsBuilder + # Load the 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"} + file_path="config/r5a_v_ros.urdf.xacro", mappings={"use_sim_time": "true"} ) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") diff --git a/src/robot_control/launch/visual_sim.launch.py b/src/src/robot_control/launch/visual_sim.launch.py similarity index 52% rename from src/robot_control/launch/visual_sim.launch.py rename to src/src/robot_control/launch/visual_sim.launch.py index eab1ad7..3ed22c4 100644 --- a/src/robot_control/launch/visual_sim.launch.py +++ b/src/src/robot_control/launch/visual_sim.launch.py @@ -1,10 +1,10 @@ """ @file visual_sim.launch.py -@brief Launch file for setting up the robot simulation with visual servoing components. +@brief Launch file for setting up the robot simulation with direct joint control. This launch file initializes the robot simulation in Gazebo with a custom world that includes a spotlight, spawns the robot entity, sets up the MoveIt configuration, and starts the necessary nodes and controllers -for the robot operation, including visual servoing components like the object detector and visual joint state publisher nodes. +for the robot operation. Visual joint state publisher has been removed as we now use direct joint angle control. """ import os @@ -14,12 +14,12 @@ IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler, - DeclareLaunchArgument, # Added to declare launch arguments + DeclareLaunchArgument, ) 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 +from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node, SetParameter import xacro from moveit_configs_utils import MoveItConfigsBuilder @@ -27,17 +27,18 @@ def generate_launch_description(): """ - @brief Generates the launch description for the robot simulation with visual servoing components. + @brief Generates the launch description for the robot simulation with direct joint control. This function sets up the robot description, launches Gazebo with a custom world file, - spawns the robot entity, configures MoveIt, and starts the necessary nodes and controllers, - including the GUI node, object detector node, and visual joint state publisher node. - - It now accepts a parameter 'num_cameras' to determine whether to launch the single or double ArUco detector. + spawns the robot entity, configures MoveIt, and starts the necessary nodes and controllers. + The visual joint state publisher has been removed as we now control joints directly. @return LaunchDescription object containing all the nodes and configurations to launch. """ - + # Force un timestamp unique pour éviter le cache + import time + timestamp = str(int(time.time())) + # Declare the 'num_cameras' launch argument num_cameras_arg = DeclareLaunchArgument( 'num_cameras', @@ -49,25 +50,15 @@ def generate_launch_description(): 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 + pkg_name = "robot_description" + robot_moveit_config = "robot_moveit_config" + share_dir = get_package_share_directory(pkg_name) + moveit_config_pkg_path = get_package_share_directory(robot_moveit_config) # Load and process URDF/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 + xacro_file = os.path.join(share_dir, "urdf", "r5a_v_ros.urdf.xacro") + robot_description_config = xacro.process_file(xacro_file) + robot_description = {"robot_description": robot_description_config.toxml()} # Robot State Publisher Node robot_state_publisher_node = Node( @@ -78,19 +69,15 @@ def generate_launch_description(): ) # Gazebo Launch - 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") gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - os.path.join( - get_package_share_directory("gazebo_ros"), - "launch", - "gazebo.launch.py", - ) - ] - ), + PythonLaunchDescriptionSource([ + os.path.join( + get_package_share_directory("gazebo_ros"), + "launch", + "gazebo.launch.py", + ) + ]), launch_arguments={"use_sim_time": "true", "world": world_file_path}.items(), ) @@ -116,7 +103,8 @@ def generate_launch_description(): os.path.join(moveit_config_pkg_path, "config", "moveit_controllers.yaml") ) .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True + publish_robot_description=True, + publish_robot_description_semantic=True ) .planning_pipelines(pipelines=["ompl"]) .to_moveit_configs() @@ -169,91 +157,86 @@ def generate_launch_description(): # Launch the GUI Node gui_node = Node( - package="robot_control", # Package name containing the GUI node - executable="moveit_control_gui", # Executable name of the GUI node + package="robot_control", + executable="moveit_control_gui", output="screen", parameters=[ moveit_config.to_dict(), {"use_sim_time": True}, - {"moveit_current_state_monitor.joint_state_qos": "sensor_data"}, - ], # Pass the MoveIt config to the GUI node + ], ) - # Aruco Detector Single Node + # ArUco Detector Single Node (optionnel - pour visualisation uniquement) aruco_detector_single_node = Node( package="robot_control", executable="aruco_detector_single", output="screen", - parameters=[ - {"use_sim_time": True}, - ], + parameters=[{"use_sim_time": True}], condition=UnlessCondition(PythonExpression(['"', num_cameras, '" == "2"'])) ) - # Aruco Detector Double Node + # ArUco Detector Double Node (optionnel - pour visualisation uniquement) aruco_detector_double_node = Node( package="robot_control", executable="aruco_detector_double", output="screen", - parameters=[ - {"use_sim_time": True}, - ], + parameters=[{"use_sim_time": True}], condition=IfCondition(PythonExpression(['"', num_cameras, '" == "2"'])) ) - # Launch Visual Joint State Publisher Node - visual_joint_state_publisher_node = Node( - package="robot_control", - executable="visual_joint_state_publisher", - output="screen", - parameters=[{"use_sim_time": True}], - ) + # SUPPRIMÉ: visual_joint_state_publisher_node n'est plus nécessaire + # Le contrôle se fait maintenant directement par angles de joints + + # SUPPRIMÉ: joint_state_publisher standard pour éviter les conflits + # Gazebo fournit déjà les joint states via joint_state_broadcaster # Return the LaunchDescription - return LaunchDescription( - [ - num_cameras_arg, # Added the launch argument - Node( - 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( - event_handler=OnProcessExit( - target_action=spawn_entity, - on_exit=[load_joint_state_controller], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=load_joint_state_controller, - on_exit=[load_arm_controller], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=load_arm_controller, - on_exit=[ - load_gripper_controller, - move_group_node - ], # Load gripper controller before move group - ) - ), - RegisterEventHandler( - event_handler=OnProcessStart( - target_action=move_group_node, - on_start=[ - gui_node, - aruco_detector_single_node, - aruco_detector_double_node, - visual_joint_state_publisher_node, - ], - ) - ), - gazebo, - robot_state_publisher_node, - spawn_entity, - ] - ) + return LaunchDescription([ + num_cameras_arg, + + # Paramètre global pour le temps de simulation + SetParameter(name="use_sim_time", value=True), + + # Séquence de lancement des composants principaux + gazebo, + robot_state_publisher_node, + spawn_entity, + + # Chargement séquentiel des contrôleurs + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_arm_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_arm_controller, + on_exit=[load_gripper_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_gripper_controller, + on_exit=[move_group_node], + ) + ), + + # Démarrage des nœuds applicatifs après MoveIt + RegisterEventHandler( + event_handler=OnProcessStart( + target_action=move_group_node, + on_start=[ + gui_node, + aruco_detector_single_node, # Optionnel - seulement pour visualisation + aruco_detector_double_node, # Optionnel - seulement pour visualisation + ], + ) + ), + ]) \ No newline at end of file diff --git a/src/robot_control/package.xml b/src/src/robot_control/package.xml similarity index 100% rename from src/robot_control/package.xml rename to src/src/robot_control/package.xml diff --git a/src/robot_control/src/aruco_detector_double.cpp b/src/src/robot_control/src/aruco_detector_double.cpp similarity index 100% rename from src/robot_control/src/aruco_detector_double.cpp rename to src/src/robot_control/src/aruco_detector_double.cpp diff --git a/src/src/robot_control/src/aruco_detector_single.cpp b/src/src/robot_control/src/aruco_detector_single.cpp new file mode 100644 index 0000000..313e759 --- /dev/null +++ b/src/src/robot_control/src/aruco_detector_single.cpp @@ -0,0 +1,239 @@ +/// @file multi_camera_viewer.cpp +/// @brief Node that displays multiple camera feeds simultaneously in a grid layout. + +#include +#include +#include +#include +#include +#include +#include + +/// @class MultiCameraViewer +/// @brief Subscribes to multiple camera topics and displays them in a grid layout. +class MultiCameraViewer : public rclcpp::Node { +public: + /// @brief Constructor for MultiCameraViewer. + MultiCameraViewer() : Node("multi_camera_viewer") { + // Initialize camera topics - adjust these to match your actual camera topics + camera_topics_ = { + "camera1/image_raw", + "camera2/image_raw", + "camera3/image_raw", + "camera4/image_raw", + "camera5/image_raw", + "camera6/image_raw", + "camera7/image_raw" + }; + + // Initialize camera names for display + camera_names_ = { + "Camera 1", "Camera 2", "Camera 3", "Camera 4", + "Camera 5", "Camera 6", "Camera 7" + }; + + // Resize vectors to hold images and status + latest_images_.resize(camera_topics_.size()); + camera_active_.resize(camera_topics_.size(), false); + + // Create subscribers for each camera + for (size_t i = 0; i < camera_topics_.size(); ++i) { + auto subscriber = this->create_subscription( + camera_topics_[i], 10, + [this, i](const sensor_msgs::msg::Image::SharedPtr msg) { + this->imageCallback(msg, i); + }); + image_subscribers_.push_back(subscriber); + + RCLCPP_INFO(this->get_logger(), "Subscribed to: %s", camera_topics_[i].c_str()); + } + + // Set display parameters + display_width_ = 1200; + display_height_ = 900; + grid_cols_ = 3; + grid_rows_ = 3; + + // Calculate individual image size for grid display + cell_width_ = display_width_ / grid_cols_; + cell_height_ = display_height_ / grid_rows_; + + // Create main display window + cv::namedWindow("Multi-Camera View", cv::WINDOW_NORMAL); + cv::resizeWindow("Multi-Camera View", display_width_, display_height_); + + // Create timer for periodic display updates + display_timer_ = this->create_wall_timer( + std::chrono::milliseconds(33), // ~30 FPS + std::bind(&MultiCameraViewer::updateDisplay, this)); + + RCLCPP_INFO(this->get_logger(), "Multi-Camera Viewer initialized"); + RCLCPP_INFO(this->get_logger(), "Grid layout: %dx%d, Cell size: %dx%d", + grid_cols_, grid_rows_, cell_width_, cell_height_); + } + +private: + /// @brief Callback function for processing received images from a specific camera. + /// @param msg The image message received. + /// @param camera_index Index of the camera that sent the image. + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg, size_t camera_index) { + try { + // Convert ROS image to OpenCV format + cv::Mat frame = cv_bridge::toCvShare(msg, "bgr8")->image; + + // Store the latest image for this camera + { + std::lock_guard lock(image_mutex_); + latest_images_[camera_index] = frame.clone(); + camera_active_[camera_index] = true; + } + + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception for camera %zu: %s", + camera_index, e.what()); + } + } + + /// @brief Creates a placeholder image for inactive cameras. + /// @param camera_index Index of the camera. + /// @return Placeholder image with camera information. + cv::Mat createPlaceholderImage(size_t camera_index) { + cv::Mat placeholder(cell_height_, cell_width_, CV_8UC3, cv::Scalar(50, 50, 50)); + + // Add text information + std::string text1 = camera_names_[camera_index]; + std::string text2 = "No Signal"; + std::string text3 = "Topic: " + camera_topics_[camera_index]; + + // Calculate text positions + int font = cv::FONT_HERSHEY_SIMPLEX; + double font_scale = 0.6; + int thickness = 2; + + cv::Size text_size1 = cv::getTextSize(text1, font, font_scale, thickness, nullptr); + cv::Size text_size2 = cv::getTextSize(text2, font, font_scale, thickness, nullptr); + cv::Size text_size3 = cv::getTextSize(text3, font, 0.4, 1, nullptr); + + cv::Point text_pos1((cell_width_ - text_size1.width) / 2, cell_height_ / 2 - 20); + cv::Point text_pos2((cell_width_ - text_size2.width) / 2, cell_height_ / 2 + 10); + cv::Point text_pos3((cell_width_ - text_size3.width) / 2, cell_height_ / 2 + 35); + + // Draw text + cv::putText(placeholder, text1, text_pos1, font, font_scale, cv::Scalar(255, 255, 255), thickness); + cv::putText(placeholder, text2, text_pos2, font, font_scale, cv::Scalar(0, 0, 255), thickness); + cv::putText(placeholder, text3, text_pos3, font, 0.4, cv::Scalar(200, 200, 200), 1); + + return placeholder; + } + + /// @brief Updates the main display with all camera feeds. + void updateDisplay() { + // Create the main display image + cv::Mat display_image(display_height_, display_width_, CV_8UC3, cv::Scalar(0, 0, 0)); + + std::lock_guard lock(image_mutex_); + + for (size_t i = 0; i < camera_topics_.size(); ++i) { + // Calculate grid position + int row = i / grid_cols_; + int col = i % grid_cols_; + + // Calculate region of interest in the display image + cv::Rect roi(col * cell_width_, row * cell_height_, cell_width_, cell_height_); + + cv::Mat cell_image; + + if (camera_active_[i] && !latest_images_[i].empty()) { + // Resize the camera image to fit the cell + cv::resize(latest_images_[i], cell_image, cv::Size(cell_width_, cell_height_)); + + // Add camera label + std::string label = camera_names_[i]; + cv::putText(cell_image, label, cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 0), 2); + + // Add timestamp + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + auto ms = std::chrono::duration_cast( + now.time_since_epoch()) % 1000; + + std::stringstream ss; + ss << std::put_time(std::localtime(&time_t), "%H:%M:%S"); + ss << "." << std::setfill('0') << std::setw(3) << ms.count(); + + cv::putText(cell_image, ss.str(), cv::Point(10, cell_height_ - 10), + cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255), 1); + } else { + // Create placeholder for inactive camera + cell_image = createPlaceholderImage(i); + } + + // Copy the cell image to the display + cell_image.copyTo(display_image(roi)); + + // Draw border around each cell + cv::rectangle(display_image, roi, cv::Scalar(100, 100, 100), 2); + } + + // Add main title + cv::putText(display_image, "Multi-Camera Monitoring System", + cv::Point(20, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, + cv::Scalar(255, 255, 255), 2); + + // Count active cameras + int active_count = std::count(camera_active_.begin(), camera_active_.end(), true); + std::string status = "Active Cameras: " + std::to_string(active_count) + "/" + + std::to_string(camera_topics_.size()); + cv::putText(display_image, status, cv::Point(display_width_ - 250, 30), + cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 255), 2); + + // Display the combined image + cv::imshow("Multi-Camera View", display_image); + + // Handle key presses + char key = cv::waitKey(1) & 0xFF; + if (key == 'q' || key == 27) { // 'q' or ESC to quit + RCLCPP_INFO(this->get_logger(), "Shutting down Multi-Camera Viewer"); + rclcpp::shutdown(); + } else if (key == 'r') { // 'r' to reset camera status + std::fill(camera_active_.begin(), camera_active_.end(), false); + RCLCPP_INFO(this->get_logger(), "Reset camera status"); + } + } + + // Member variables + std::vector camera_topics_; + std::vector camera_names_; + std::vector::SharedPtr> image_subscribers_; + std::vector latest_images_; + std::vector camera_active_; + + std::mutex image_mutex_; + rclcpp::TimerBase::SharedPtr display_timer_; + + int display_width_, display_height_; + int grid_cols_, grid_rows_; + int cell_width_, cell_height_; +}; + +/// @brief Main function that initializes and spins the MultiCameraViewer node. +/// @param argc Argument count. +/// @param argv Argument vector. +/// @return Exit status code. +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + RCLCPP_INFO(node->get_logger(), "Starting Multi-Camera Viewer"); + RCLCPP_INFO(node->get_logger(), "Press 'q' or ESC to quit"); + RCLCPP_INFO(node->get_logger(), "Press 'r' to reset camera status"); + + rclcpp::spin(node); + + // Cleanup + cv::destroyAllWindows(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/robot_control/src/aruco_rotation_publisher.cpp b/src/src/robot_control/src/aruco_rotation_publisher.cpp similarity index 100% rename from src/robot_control/src/aruco_rotation_publisher.cpp rename to src/src/robot_control/src/aruco_rotation_publisher.cpp diff --git a/src/robot_control/src/cam_calibration.cpp b/src/src/robot_control/src/cam_calibration.cpp similarity index 100% rename from src/robot_control/src/cam_calibration.cpp rename to src/src/robot_control/src/cam_calibration.cpp diff --git a/src/robot_control/src/end_pose_node.cpp b/src/src/robot_control/src/end_pose_node.cpp similarity index 100% rename from src/robot_control/src/end_pose_node.cpp rename to src/src/robot_control/src/end_pose_node.cpp diff --git a/src/robot_control/src/moveit_control_gui.cpp b/src/src/robot_control/src/moveit_control_gui.cpp similarity index 68% rename from src/robot_control/src/moveit_control_gui.cpp rename to src/src/robot_control/src/moveit_control_gui.cpp index 8b7b68a..3f0e45b 100644 --- a/src/robot_control/src/moveit_control_gui.cpp +++ b/src/src/robot_control/src/moveit_control_gui.cpp @@ -40,12 +40,15 @@ class MoveItControlGui : public QWidget { // GUI Setup QVBoxLayout *layout = new QVBoxLayout(this); + // Control buttons QPushButton *move_to_home_btn = new QPushButton("Move to Home Position", this); - QPushButton *move_to_predefined_btn = new QPushButton("Move to Predefined Position", this); - QPushButton *move_to_random_pose_btn = new QPushButton("Move to Random Pose", this); - QPushButton *move_to_tf_btn = new QPushButton("Move to TF Position", this); QPushButton *refresh_frames_btn = new QPushButton("Refresh TF Frames", this); + // Joint control buttons + QPushButton *move_l1l2_90deg_x_btn = new QPushButton("Link1-Link2 at 90° (X)", this); + QPushButton *move_l1l2_90deg_y_btn = new QPushButton("Link1-Link2 at 90° (Y)", this); + + // Gripper control buttons QPushButton *open_gripper_btn = new QPushButton("Open Gripper", this); QPushButton *close_gripper_btn = new QPushButton("Close Gripper", this); @@ -55,19 +58,19 @@ class MoveItControlGui : public QWidget { status_label_ = new QLabel("Status: Waiting for action", this); layout->addWidget(status_label_); + // Add buttons to layout layout->addWidget(move_to_home_btn); - layout->addWidget(move_to_predefined_btn); - layout->addWidget(move_to_random_pose_btn); - layout->addWidget(move_to_tf_btn); layout->addWidget(refresh_frames_btn); + layout->addWidget(move_l1l2_90deg_x_btn); + layout->addWidget(move_l1l2_90deg_y_btn); layout->addWidget(open_gripper_btn); layout->addWidget(close_gripper_btn); + // Connect signals connect(move_to_home_btn, &QPushButton::clicked, this, &MoveItControlGui::moveToHomePosition); - connect(move_to_predefined_btn, &QPushButton::clicked, this, &MoveItControlGui::moveToPredefinedPosition); - connect(move_to_random_pose_btn, &QPushButton::clicked, this, &MoveItControlGui::moveToRandomPose); - connect(move_to_tf_btn, &QPushButton::clicked, this, &MoveItControlGui::moveToTf); connect(refresh_frames_btn, &QPushButton::clicked, this, &MoveItControlGui::refreshTfFrames); + connect(move_l1l2_90deg_x_btn, &QPushButton::clicked, this, &MoveItControlGui::moveToL1L2_90Degree_x_Position); + connect(move_l1l2_90deg_y_btn, &QPushButton::clicked, this, &MoveItControlGui::moveToL1L2_90Degree_y_Position); connect(open_gripper_btn, &QPushButton::clicked, this, &MoveItControlGui::openGripper); connect(close_gripper_btn, &QPushButton::clicked, this, &MoveItControlGui::closeGripper); @@ -93,55 +96,73 @@ class MoveItControlGui : public QWidget { rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::thread executor_thread_; + // Home position using cartesian coordinates void moveToHomePosition() { moveToPosition(0.006755, 0.000006, 0.777373, 0.0, 0.0, 1.0, 0.0); } - void moveToPredefinedPosition() { - moveToPosition(0.37441, 0.12162, 0.51234, 0.61579, 0.77367, 0.072197, 0.13048); + void refreshTfFrames() { + // Logic to refresh the TF frames in the combo box + tf_frame_selector_->clear(); + // Populate tf_frame_selector_ with available frames } -void moveToRandomPose() { - // Seed the random number generator - static bool seeded = false; - if (!seeded) { - srand(static_cast(time(0))); // Seed with current time - seeded = true; + // Joint control functions + void moveToJointPosition(const std::vector& joint_angles) { + // Définir les valeurs des joints (en radians) + std::vector joint_names = { + "R0_Yaw", // Rotation base (yaw) + "R1_Pitch", // Angle entre link1 et link2 (pitch) + "R2_Pitch", // Angle entre link2 et link3 (pitch) + "R3_Yaw", // Rotation link3-link4 (yaw) + "R4_Pitch" // Angle entre link4 et link5 (pitch) + }; + + if (joint_angles.size() != joint_names.size()) { + RCLCPP_ERROR(node_->get_logger(), "Number of joint angles doesn't match number of joints!"); + status_label_->setText("Status: Joint angle count mismatch!"); + return; + } + + // Utiliser setJointValueTarget au lieu de setPoseTarget + move_group_interface_.setJointValueTarget(joint_names, joint_angles); + + // Planifier et exécuter le mouvement + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + if (move_group_interface_.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_INFO(node_->get_logger(), "Joint plan successful! Executing joint movement..."); + move_group_interface_.execute(my_plan); + status_label_->setText("Status: Moved to joint position successfully"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Joint planning failed!"); + status_label_->setText("Status: Joint planning failed!"); + } } - // Define limits for random pose generation - double x_min = 0.3; // Minimum x position - double x_max = 0.8; // Maximum x position - double y_min = 0.3; // Minimum y position - double y_max = 0.8; // Maximum y position - double z_min = 0.3; // Minimum z position (e.g., ground level) - double z_max = 0.8; // Maximum z position (e.g., above ground) - - // Generate random pose within specified limits - double random_x = x_min + static_cast(rand()) / (static_cast(RAND_MAX / (x_max - x_min))); - double random_y = y_min + static_cast(rand()) / (static_cast(RAND_MAX / (y_max - y_min))); - double random_z = z_min + static_cast(rand()) / (static_cast(RAND_MAX / (z_max - z_min))); - - - // Define a random orientation (roll, pitch, yaw) - double random_roll = static_cast(rand()) / (static_cast(RAND_MAX / (2.0 * M_PI))); // You can generate random values if needed - double random_pitch = static_cast(rand()) / (static_cast(RAND_MAX / (2.0 * M_PI))); // You can generate random values if needed - double random_yaw = static_cast(rand()) / (static_cast(RAND_MAX / (2.0 * M_PI))); // Random yaw between 0 and 2π - double random_w = static_cast(rand()) / (static_cast(RAND_MAX / (2.0 * M_PI))); // Random yaw between 0 and 2π - - // Move to the generated random pose - moveToPosition(random_x, random_y, random_z, random_roll, random_pitch, random_yaw, random_w); -} - - void moveToTf() { - // Logic to move to a pose based on TF frame selection - std::string selected_frame = tf_frame_selector_->currentText().toStdString(); - // Retrieve the transform and move to the corresponding pose + + // Position avec angle de 90° entre link1 et link2 (mouvement en Y) + void moveToL1L2_90Degree_y_Position() { + RCLCPP_INFO(node_->get_logger(), "Moving to Link1-Link2 90° position (Y axis)..."); + std::vector joint_angles = { + 0.0, // R0_Yaw : 0° + -M_PI/2, // R1_Pitch : -90° (pour lever le bras) + 0.0, // R2_Pitch : 0° + 0.0, // R3_Yaw : 0° + 0.0 // R4_Pitch : 0° + }; + moveToJointPosition(joint_angles); } - void refreshTfFrames() { - // Logic to refresh the TF frames in the combo box - tf_frame_selector_->clear(); - // Populate tf_frame_selector_ with available frames + // Position avec angle de 90° entre link1 et link2 (mouvement en X) + void moveToL1L2_90Degree_x_Position() { + RCLCPP_INFO(node_->get_logger(), "Moving to Link1-Link2 90° position (X axis)..."); + std::vector joint_angles = { + M_PI/2, // R0_Yaw : 90° (rotation de la base) + -M_PI/2, // R1_Pitch : -90° (pour lever le bras) + 0.0, // R2_Pitch : 0° + 0.0, // R3_Yaw : 0° + 0.0 // R4_Pitch : 0° + }; + moveToJointPosition(joint_angles); } // Constants for gripper positions @@ -228,7 +249,6 @@ void moveToRandomPose() { move_group_interface_.move(); status_label_->setText("Status: Moved to target position"); } - }; int main(int argc, char **argv) { @@ -238,4 +258,4 @@ int main(int argc, char **argv) { MoveItControlGui gui(node); gui.show(); return app.exec(); -} +} \ No newline at end of file diff --git a/src/robot_control/src/moveit_control_simple.cpp b/src/src/robot_control/src/moveit_control_simple.cpp similarity index 100% rename from src/robot_control/src/moveit_control_simple.cpp rename to src/src/robot_control/src/moveit_control_simple.cpp diff --git a/src/robot_control/src/visual_joint_state_publisher.cpp b/src/src/robot_control/src/visual_joint_state_publisher.cpp similarity index 100% rename from src/robot_control/src/visual_joint_state_publisher.cpp rename to src/src/robot_control/src/visual_joint_state_publisher.cpp diff --git a/src/robot_description/CMakeLists.txt b/src/src/robot_description/CMakeLists.txt similarity index 70% rename from src/robot_description/CMakeLists.txt rename to src/src/robot_description/CMakeLists.txt index 8f1cd2e..7466ae8 100644 --- a/src/robot_description/CMakeLists.txt +++ b/src/src/robot_description/CMakeLists.txt @@ -22,4 +22,12 @@ install( DESTINATION share/${PROJECT_NAME}/urdf ) -ament_package() +# Installer les matériaux +install(DIRECTORY media/materials/ + DESTINATION share/${PROJECT_NAME}/materials/) + +# Installer les mondes avec les fichiers SDF (inclut custom_world_with_spotlight.sdf) +install(DIRECTORY worlds/ + DESTINATION share/${PROJECT_NAME}/worlds/) + +ament_package() \ No newline at end of file diff --git a/src/robot_description/config/aruco_planes.rviz b/src/src/robot_description/config/aruco_planes.rviz similarity index 100% rename from src/robot_description/config/aruco_planes.rviz rename to src/src/robot_description/config/aruco_planes.rviz diff --git a/src/robot_description/config/controller.yaml b/src/src/robot_description/config/controller.yaml similarity index 100% rename from src/robot_description/config/controller.yaml rename to src/src/robot_description/config/controller.yaml diff --git a/src/robot_description/launch/gazebo.launch.py b/src/src/robot_description/launch/gazebo.launch.py similarity index 80% rename from src/robot_description/launch/gazebo.launch.py rename to src/src/robot_description/launch/gazebo.launch.py index f5ac216..be21b40 100644 --- a/src/robot_description/launch/gazebo.launch.py +++ b/src/src/robot_description/launch/gazebo.launch.py @@ -1,9 +1,10 @@ """ @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 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 a custom world that includes +spotlight illumination, spawns the robot entity, and starts the necessary nodes and controllers +for the robot operation. """ import os @@ -26,8 +27,8 @@ 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 world lighting, + spawns the robot entity, and starts the necessary nodes and controllers. @return LaunchDescription object containing all the nodes and configurations to launch. """ @@ -47,6 +48,10 @@ def generate_launch_description(): robot_description_xacro.toxml() ) # Convert the processed XACRO to URDF XML + # Path to the custom world file with spotlight illumination + world_file_name = "custom_world_with_spotlight.sdf" + world_path = os.path.join(share_dir, "worlds", world_file_name) + # Configure the robot_state_publisher node node_robot_state_publisher = Node( package="robot_state_publisher", # Package containing the node @@ -58,7 +63,7 @@ def generate_launch_description(): ], # Parameters ) - # Node to spawn the entity in Gazebo + # Node to spawn the entity in Gazebo at position (0,0,0) spawn_entity = Node( package="gazebo_ros", # Package containing the node executable="spawn_entity.py", # Executable script to spawn entities @@ -67,11 +72,14 @@ def generate_launch_description(): "/robot_description", "-entity", "armr5", - ], # Arguments for spawning + "-x", "0.0", + "-y", "0.0", + "-z", "0.0", + ], # Arguments for spawning at origin (0,0,0) output="screen", ) - # Include the Gazebo launch file + # Include the Gazebo launch file with custom world gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -79,6 +87,10 @@ def generate_launch_description(): "/gazebo.launch.py", ] ), + launch_arguments={ + "world": world_path, + "verbose": "true", + }.items(), ) # Commands to load and start controllers after spawning the robot @@ -125,4 +137,4 @@ def generate_launch_description(): ) ), ] - ) + ) \ No newline at end of file diff --git a/src/robot_description/launch/gazebo_move.launch.py b/src/src/robot_description/launch/gazebo_move.launch.py similarity index 98% rename from src/robot_description/launch/gazebo_move.launch.py rename to src/src/robot_description/launch/gazebo_move.launch.py index 9b8d312..6e55f44 100644 --- a/src/robot_description/launch/gazebo_move.launch.py +++ b/src/src/robot_description/launch/gazebo_move.launch.py @@ -111,7 +111,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("robot_moveit_config", package_name="robot_moveit_config") .robot_description( - file_path=xacro_file, mappings={"use_sim_time": "true"} + file_path="config/r5a_v_ros.urdf.xacro", mappings={"use_sim_time": "true"} ) .robot_description_semantic("config/armr5.srdf") .robot_description_kinematics("config/kinematics.yaml") diff --git a/src/robot_description/media/materials/scripts/aruco_plane.material b/src/src/robot_description/media/materials/scripts/aruco_plane.material similarity index 100% rename from src/robot_description/media/materials/scripts/aruco_plane.material rename to src/src/robot_description/media/materials/scripts/aruco_plane.material diff --git a/src/src/robot_description/media/materials/scripts/robot_materials_PLA.material b/src/src/robot_description/media/materials/scripts/robot_materials_PLA.material new file mode 100644 index 0000000..70a5c1a --- /dev/null +++ b/src/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/src/robot_description/media/materials/textures/id0.png similarity index 100% rename from src/robot_description/media/materials/textures/id0.png rename to src/src/robot_description/media/materials/textures/id0.png diff --git a/src/robot_description/media/materials/textures/id1.png b/src/src/robot_description/media/materials/textures/id1.png similarity index 100% rename from src/robot_description/media/materials/textures/id1.png rename to src/src/robot_description/media/materials/textures/id1.png diff --git a/src/robot_description/media/materials/textures/id10.png b/src/src/robot_description/media/materials/textures/id10.png similarity index 100% rename from src/robot_description/media/materials/textures/id10.png rename to src/src/robot_description/media/materials/textures/id10.png diff --git a/src/robot_description/media/materials/textures/id11.png b/src/src/robot_description/media/materials/textures/id11.png similarity index 100% rename from src/robot_description/media/materials/textures/id11.png rename to src/src/robot_description/media/materials/textures/id11.png diff --git a/src/robot_description/media/materials/textures/id12.png b/src/src/robot_description/media/materials/textures/id12.png similarity index 100% rename from src/robot_description/media/materials/textures/id12.png rename to src/src/robot_description/media/materials/textures/id12.png diff --git a/src/robot_description/media/materials/textures/id13.png b/src/src/robot_description/media/materials/textures/id13.png similarity index 100% rename from src/robot_description/media/materials/textures/id13.png rename to src/src/robot_description/media/materials/textures/id13.png diff --git a/src/robot_description/media/materials/textures/id14.png b/src/src/robot_description/media/materials/textures/id14.png similarity index 100% rename from src/robot_description/media/materials/textures/id14.png rename to src/src/robot_description/media/materials/textures/id14.png diff --git a/src/robot_description/media/materials/textures/id15.png b/src/src/robot_description/media/materials/textures/id15.png similarity index 100% rename from src/robot_description/media/materials/textures/id15.png rename to src/src/robot_description/media/materials/textures/id15.png diff --git a/src/robot_description/media/materials/textures/id16.png b/src/src/robot_description/media/materials/textures/id16.png similarity index 100% rename from src/robot_description/media/materials/textures/id16.png rename to src/src/robot_description/media/materials/textures/id16.png diff --git a/src/robot_description/media/materials/textures/id17.png b/src/src/robot_description/media/materials/textures/id17.png similarity index 100% rename from src/robot_description/media/materials/textures/id17.png rename to src/src/robot_description/media/materials/textures/id17.png diff --git a/src/robot_description/media/materials/textures/id18.png b/src/src/robot_description/media/materials/textures/id18.png similarity index 100% rename from src/robot_description/media/materials/textures/id18.png rename to src/src/robot_description/media/materials/textures/id18.png diff --git a/src/robot_description/media/materials/textures/id19.png b/src/src/robot_description/media/materials/textures/id19.png similarity index 100% rename from src/robot_description/media/materials/textures/id19.png rename to src/src/robot_description/media/materials/textures/id19.png diff --git a/src/robot_description/media/materials/textures/id2.png b/src/src/robot_description/media/materials/textures/id2.png similarity index 100% rename from src/robot_description/media/materials/textures/id2.png rename to src/src/robot_description/media/materials/textures/id2.png diff --git a/src/robot_description/media/materials/textures/id3.png b/src/src/robot_description/media/materials/textures/id3.png similarity index 100% rename from src/robot_description/media/materials/textures/id3.png rename to src/src/robot_description/media/materials/textures/id3.png diff --git a/src/robot_description/media/materials/textures/id4.png b/src/src/robot_description/media/materials/textures/id4.png similarity index 100% rename from src/robot_description/media/materials/textures/id4.png rename to src/src/robot_description/media/materials/textures/id4.png diff --git a/src/robot_description/media/materials/textures/id5.png b/src/src/robot_description/media/materials/textures/id5.png similarity index 100% rename from src/robot_description/media/materials/textures/id5.png rename to src/src/robot_description/media/materials/textures/id5.png diff --git a/src/robot_description/media/materials/textures/id6.png b/src/src/robot_description/media/materials/textures/id6.png similarity index 100% rename from src/robot_description/media/materials/textures/id6.png rename to src/src/robot_description/media/materials/textures/id6.png diff --git a/src/robot_description/media/materials/textures/id7.png b/src/src/robot_description/media/materials/textures/id7.png similarity index 100% rename from src/robot_description/media/materials/textures/id7.png rename to src/src/robot_description/media/materials/textures/id7.png diff --git a/src/robot_description/media/materials/textures/id8.png b/src/src/robot_description/media/materials/textures/id8.png similarity index 100% rename from src/robot_description/media/materials/textures/id8.png rename to src/src/robot_description/media/materials/textures/id8.png diff --git a/src/robot_description/media/materials/textures/id9.png b/src/src/robot_description/media/materials/textures/id9.png similarity index 100% rename from src/robot_description/media/materials/textures/id9.png rename to src/src/robot_description/media/materials/textures/id9.png diff --git a/src/robot_description/meshes/3dCamera.DAE b/src/src/robot_description/meshes/3dCamera.DAE similarity index 100% rename from src/robot_description/meshes/3dCamera.DAE rename to src/src/robot_description/meshes/3dCamera.DAE diff --git a/src/robot_description/meshes/3dCamera.png b/src/src/robot_description/meshes/3dCamera.png similarity index 100% rename from src/robot_description/meshes/3dCamera.png rename to src/src/robot_description/meshes/3dCamera.png diff --git a/src/robot_description/meshes/R5A_GripperLeft_Link1.STL b/src/src/robot_description/meshes/R5A_GripperLeft_Link1.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperLeft_Link1.STL rename to src/src/robot_description/meshes/R5A_GripperLeft_Link1.STL diff --git a/src/robot_description/meshes/R5A_GripperLeft_Link2.STL b/src/src/robot_description/meshes/R5A_GripperLeft_Link2.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperLeft_Link2.STL rename to src/src/robot_description/meshes/R5A_GripperLeft_Link2.STL diff --git a/src/robot_description/meshes/R5A_GripperLeft_Link3.STL b/src/src/robot_description/meshes/R5A_GripperLeft_Link3.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperLeft_Link3.STL rename to src/src/robot_description/meshes/R5A_GripperLeft_Link3.STL diff --git a/src/robot_description/meshes/R5A_GripperLeft_Link31.STL b/src/src/robot_description/meshes/R5A_GripperLeft_Link31.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperLeft_Link31.STL rename to src/src/robot_description/meshes/R5A_GripperLeft_Link31.STL diff --git a/src/robot_description/meshes/R5A_GripperRight_Link1.STL b/src/src/robot_description/meshes/R5A_GripperRight_Link1.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperRight_Link1.STL rename to src/src/robot_description/meshes/R5A_GripperRight_Link1.STL diff --git a/src/robot_description/meshes/R5A_GripperRight_Link2.STL b/src/src/robot_description/meshes/R5A_GripperRight_Link2.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperRight_Link2.STL rename to src/src/robot_description/meshes/R5A_GripperRight_Link2.STL diff --git a/src/robot_description/meshes/R5A_GripperRight_Link3.STL b/src/src/robot_description/meshes/R5A_GripperRight_Link3.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperRight_Link3.STL rename to src/src/robot_description/meshes/R5A_GripperRight_Link3.STL diff --git a/src/robot_description/meshes/R5A_GripperRight_Link31.STL b/src/src/robot_description/meshes/R5A_GripperRight_Link31.STL similarity index 100% rename from src/robot_description/meshes/R5A_GripperRight_Link31.STL rename to src/src/robot_description/meshes/R5A_GripperRight_Link31.STL diff --git a/src/robot_description/meshes/R5A_link1.STL b/src/src/robot_description/meshes/R5A_link1.STL similarity index 100% rename from src/robot_description/meshes/R5A_link1.STL rename to src/src/robot_description/meshes/R5A_link1.STL diff --git a/src/robot_description/meshes/R5A_link2.STL b/src/src/robot_description/meshes/R5A_link2.STL similarity index 100% rename from src/robot_description/meshes/R5A_link2.STL rename to src/src/robot_description/meshes/R5A_link2.STL diff --git a/src/robot_description/meshes/R5A_link3.STL b/src/src/robot_description/meshes/R5A_link3.STL similarity index 100% rename from src/robot_description/meshes/R5A_link3.STL rename to src/src/robot_description/meshes/R5A_link3.STL diff --git a/src/robot_description/meshes/R5A_link4.STL b/src/src/robot_description/meshes/R5A_link4.STL similarity index 100% rename from src/robot_description/meshes/R5A_link4.STL rename to src/src/robot_description/meshes/R5A_link4.STL diff --git a/src/robot_description/meshes/R5A_link5.STL b/src/src/robot_description/meshes/R5A_link5.STL similarity index 100% rename from src/robot_description/meshes/R5A_link5.STL rename to src/src/robot_description/meshes/R5A_link5.STL diff --git a/src/robot_description/meshes/base_link.STL b/src/src/robot_description/meshes/base_link.STL similarity index 100% rename from src/robot_description/meshes/base_link.STL rename to src/src/robot_description/meshes/base_link.STL diff --git a/src/robot_description/models/aruco_block_long/meshes/Block_Long.png b/src/src/robot_description/models/aruco_block_long/meshes/Block_Long.png old mode 100755 new mode 100644 similarity index 100% rename from src/robot_description/models/aruco_block_long/meshes/Block_Long.png rename to src/src/robot_description/models/aruco_block_long/meshes/Block_Long.png diff --git a/src/robot_description/models/aruco_block_long/meshes/block_long.dae b/src/src/robot_description/models/aruco_block_long/meshes/block_long.dae old mode 100755 new mode 100644 similarity index 100% rename from src/robot_description/models/aruco_block_long/meshes/block_long.dae rename to src/src/robot_description/models/aruco_block_long/meshes/block_long.dae diff --git a/src/robot_description/models/aruco_block_long/meshes/block_long.stl b/src/src/robot_description/models/aruco_block_long/meshes/block_long.stl old mode 100755 new mode 100644 similarity index 100% rename from src/robot_description/models/aruco_block_long/meshes/block_long.stl rename to src/src/robot_description/models/aruco_block_long/meshes/block_long.stl diff --git a/src/robot_description/models/aruco_block_long/model.config b/src/src/robot_description/models/aruco_block_long/model.config old mode 100755 new mode 100644 similarity index 100% rename from src/robot_description/models/aruco_block_long/model.config rename to src/src/robot_description/models/aruco_block_long/model.config diff --git a/src/robot_description/models/aruco_block_long/model.sdf b/src/src/robot_description/models/aruco_block_long/model.sdf old mode 100755 new mode 100644 similarity index 100% rename from src/robot_description/models/aruco_block_long/model.sdf rename to src/src/robot_description/models/aruco_block_long/model.sdf diff --git a/src/robot_description/models/checkerboard/checkerboard_plane.sdf b/src/src/robot_description/models/checkerboard/checkerboard_plane.sdf similarity index 100% rename from src/robot_description/models/checkerboard/checkerboard_plane.sdf rename to src/src/robot_description/models/checkerboard/checkerboard_plane.sdf diff --git a/src/robot_description/models/checkerboard/materials/textures/checker.png b/src/src/robot_description/models/checkerboard/materials/textures/checker.png similarity index 100% rename from src/robot_description/models/checkerboard/materials/textures/checker.png rename to src/src/robot_description/models/checkerboard/materials/textures/checker.png diff --git a/src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae b/src/src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae similarity index 100% rename from src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae rename to src/src/robot_description/models/checkerboard/meshes/checkerboard_plane.dae diff --git a/src/robot_description/models/checkerboard/model.config b/src/src/robot_description/models/checkerboard/model.config similarity index 100% rename from src/robot_description/models/checkerboard/model.config rename to src/src/robot_description/models/checkerboard/model.config diff --git a/src/robot_description/package.xml b/src/src/robot_description/package.xml similarity index 92% rename from src/robot_description/package.xml rename to src/src/robot_description/package.xml index 2dc8a57..2dbd3ff 100644 --- a/src/robot_description/package.xml +++ b/src/src/robot_description/package.xml @@ -1,30 +1,30 @@ - - - robot_description - 0.0.1 - The description package for the armr5 robot - Your Name - - Apache-2.0 - - ament_cmake - - - rclcpp - urdf - xacro - gazebo_ros - ros2_control - controller_manager - joint_state_controller - joint_trajectory_controller - robot_state_publisher - - rviz2 - - - ament_cmake - - - - + + + robot_description + 0.0.1 + The description package for the armr5 robot + Your Name + + Apache-2.0 + + ament_cmake + + + rclcpp + urdf + xacro + gazebo_ros + ros2_control + controller_manager + joint_state_broadcaster --> + joint_trajectory_controller + robot_state_publisher + + rviz2 + + + ament_cmake + + + + diff --git a/src/robot_description/urdf/r5a_v_ros.urdf.xacro b/src/src/robot_description/urdf/r5a_v_ros.urdf.xacro similarity index 64% rename from src/robot_description/urdf/r5a_v_ros.urdf.xacro rename to src/src/robot_description/urdf/r5a_v_ros.urdf.xacro index 0512b25..ba0bb2f 100644 --- a/src/robot_description/urdf/r5a_v_ros.urdf.xacro +++ b/src/src/robot_description/urdf/r5a_v_ros.urdf.xacro @@ -1,6 +1,7 @@ - + + @@ -101,8 +102,8 @@ @@ -160,7 +161,7 @@ xyz="-0.999934112328875 0 -0.011479155067573" /> @@ -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 @@ -883,20 +914,21 @@ - + - + + - 30.0 + 60.0 - 1.3962634 + 1.0855 - 1920 - 1080 + 1600 + 1200 R8G8B8 @@ -916,7 +948,7 @@ image_raw camera_info camera_link1 - 0.07 + 0.0 0.0 0.0 @@ -926,7 +958,8 @@ - + + @@ -947,25 +980,26 @@ - + - + + - 30.0 + 60.0 - 1.3962634 + 1.0855 - 1920 - 1080 + 1600 + 1200 R8G8B8 0.02 - 300 + 300 gaussian @@ -977,10 +1011,77 @@ true 0.0 mybot/camera2 - image_raw2 - camera_info2 + image_raw + camera_info camera_link2 - 0.07 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + 60.0 + + 1.0855 + + 1600 + 1200 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + mybot/camera3 + image_raw + camera_info + camera_link3 + 0.0 0.0 0.0 @@ -990,4 +1091,274 @@ - + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + 60.0 + + 1.0855 + + 1600 + 1200 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + mybot/camera4 + image_raw + camera_info + camera_link4 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + 60.0 + + 1.0855 + + 1600 + 1200 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + mybot/camera5 + image_raw + camera_info + camera_link5 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + 60.0 + + 1.0855 + + 1600 + 1200 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + mybot/camera6 + image_raw + camera_info + camera_link6 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + 60.0 + + 1.0855 + + 1600 + 1200 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + mybot/camera7 + image_raw + camera_info + camera_link7 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + \ No newline at end of file diff --git a/src/robot_description/worlds/aruco_detection.world b/src/src/robot_description/worlds/aruco_detection.world similarity index 100% rename from src/robot_description/worlds/aruco_detection.world rename to src/src/robot_description/worlds/aruco_detection.world diff --git a/src/robot_description/worlds/spotlight.world b/src/src/robot_description/worlds/spotlight.world similarity index 100% rename from src/robot_description/worlds/spotlight.world rename to src/src/robot_description/worlds/spotlight.world diff --git a/src/robot_moveit_config/.setup_assistant b/src/src/robot_moveit_config/.setup_assistant similarity index 100% rename from src/robot_moveit_config/.setup_assistant rename to src/src/robot_moveit_config/.setup_assistant diff --git a/src/robot_moveit_config/CMakeLists.txt b/src/src/robot_moveit_config/CMakeLists.txt similarity index 100% rename from src/robot_moveit_config/CMakeLists.txt rename to src/src/robot_moveit_config/CMakeLists.txt diff --git a/src/robot_moveit_config/config/armr5.ros2_control.xacro b/src/src/robot_moveit_config/config/armr5.ros2_control.xacro similarity index 100% rename from src/robot_moveit_config/config/armr5.ros2_control.xacro rename to src/src/robot_moveit_config/config/armr5.ros2_control.xacro diff --git a/src/src/robot_moveit_config/config/armr5.srdf b/src/src/robot_moveit_config/config/armr5.srdf new file mode 100644 index 0000000..64f2ab7 --- /dev/null +++ b/src/src/robot_moveit_config/config/armr5.srdf @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_moveit_config/config/armr5.srdf b/src/src/robot_moveit_config/config/armr5.srdf.backup_20250722_171608 similarity index 100% rename from src/robot_moveit_config/config/armr5.srdf rename to src/src/robot_moveit_config/config/armr5.srdf.backup_20250722_171608 diff --git a/src/src/robot_moveit_config/config/armr5.srdf.backup_20250722_171617 b/src/src/robot_moveit_config/config/armr5.srdf.backup_20250722_171617 new file mode 100644 index 0000000..64f2ab7 --- /dev/null +++ b/src/src/robot_moveit_config/config/armr5.srdf.backup_20250722_171617 @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robot_moveit_config/config/armr5.urdf.xacro b/src/src/robot_moveit_config/config/armr5.urdf.xacro similarity index 100% rename from src/robot_moveit_config/config/armr5.urdf.xacro rename to src/src/robot_moveit_config/config/armr5.urdf.xacro diff --git a/src/robot_moveit_config/config/initial_positions.yaml b/src/src/robot_moveit_config/config/initial_positions.yaml similarity index 100% rename from src/robot_moveit_config/config/initial_positions.yaml rename to src/src/robot_moveit_config/config/initial_positions.yaml diff --git a/src/robot_moveit_config/config/joint_limits.yaml b/src/src/robot_moveit_config/config/joint_limits.yaml similarity index 100% rename from src/robot_moveit_config/config/joint_limits.yaml rename to src/src/robot_moveit_config/config/joint_limits.yaml diff --git a/src/robot_moveit_config/config/kinematics.yaml b/src/src/robot_moveit_config/config/kinematics.yaml similarity index 100% rename from src/robot_moveit_config/config/kinematics.yaml rename to src/src/robot_moveit_config/config/kinematics.yaml diff --git a/src/robot_moveit_config/config/moveit.rviz b/src/src/robot_moveit_config/config/moveit.rviz similarity index 100% rename from src/robot_moveit_config/config/moveit.rviz rename to src/src/robot_moveit_config/config/moveit.rviz diff --git a/src/robot_moveit_config/config/moveit_controllers.yaml b/src/src/robot_moveit_config/config/moveit_controllers.yaml similarity index 100% rename from src/robot_moveit_config/config/moveit_controllers.yaml rename to src/src/robot_moveit_config/config/moveit_controllers.yaml diff --git a/src/robot_moveit_config/config/pilz_cartesian_limits.yaml b/src/src/robot_moveit_config/config/pilz_cartesian_limits.yaml similarity index 100% rename from src/robot_moveit_config/config/pilz_cartesian_limits.yaml rename to src/src/robot_moveit_config/config/pilz_cartesian_limits.yaml diff --git a/src/robot_moveit_config/config/ros2_controllers.yaml b/src/src/robot_moveit_config/config/ros2_controllers.yaml similarity index 100% rename from src/robot_moveit_config/config/ros2_controllers.yaml rename to src/src/robot_moveit_config/config/ros2_controllers.yaml diff --git a/src/robot_moveit_config/config/sensors_3d.yaml b/src/src/robot_moveit_config/config/sensors_3d.yaml similarity index 100% rename from src/robot_moveit_config/config/sensors_3d.yaml rename to src/src/robot_moveit_config/config/sensors_3d.yaml diff --git a/src/robot_moveit_config/launch/demo.launch.py b/src/src/robot_moveit_config/launch/demo.launch.py similarity index 100% rename from src/robot_moveit_config/launch/demo.launch.py rename to src/src/robot_moveit_config/launch/demo.launch.py diff --git a/src/robot_moveit_config/launch/move_group.launch.py b/src/src/robot_moveit_config/launch/move_group.launch.py similarity index 100% rename from src/robot_moveit_config/launch/move_group.launch.py rename to src/src/robot_moveit_config/launch/move_group.launch.py diff --git a/src/robot_moveit_config/launch/moveit_rviz.launch.py b/src/src/robot_moveit_config/launch/moveit_rviz.launch.py similarity index 100% rename from src/robot_moveit_config/launch/moveit_rviz.launch.py rename to src/src/robot_moveit_config/launch/moveit_rviz.launch.py diff --git a/src/robot_moveit_config/launch/rsp.launch.py b/src/src/robot_moveit_config/launch/rsp.launch.py similarity index 100% rename from src/robot_moveit_config/launch/rsp.launch.py rename to src/src/robot_moveit_config/launch/rsp.launch.py diff --git a/src/robot_moveit_config/launch/setup_assistant.launch.py b/src/src/robot_moveit_config/launch/setup_assistant.launch.py similarity index 100% rename from src/robot_moveit_config/launch/setup_assistant.launch.py rename to src/src/robot_moveit_config/launch/setup_assistant.launch.py diff --git a/src/robot_moveit_config/launch/spawn_controllers.launch.py b/src/src/robot_moveit_config/launch/spawn_controllers.launch.py similarity index 100% rename from src/robot_moveit_config/launch/spawn_controllers.launch.py rename to src/src/robot_moveit_config/launch/spawn_controllers.launch.py diff --git a/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py b/src/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py similarity index 100% rename from src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py rename to src/src/robot_moveit_config/launch/static_virtual_joint_tfs.launch.py diff --git a/src/robot_moveit_config/launch/warehouse_db.launch.py b/src/src/robot_moveit_config/launch/warehouse_db.launch.py similarity index 100% rename from src/robot_moveit_config/launch/warehouse_db.launch.py rename to src/src/robot_moveit_config/launch/warehouse_db.launch.py diff --git a/src/robot_moveit_config/package.xml b/src/src/robot_moveit_config/package.xml similarity index 100% rename from src/robot_moveit_config/package.xml rename to src/src/robot_moveit_config/package.xml diff --git a/src/robot_test/CMakeLists.txt b/src/src/robot_test/CMakeLists.txt similarity index 100% rename from src/robot_test/CMakeLists.txt rename to src/src/robot_test/CMakeLists.txt diff --git a/src/robot_test/data_analysis/requirements.txt b/src/src/robot_test/data_analysis/requirements.txt similarity index 100% rename from src/robot_test/data_analysis/requirements.txt rename to src/src/robot_test/data_analysis/requirements.txt diff --git a/src/robot_test/data_analysis/scripts/generate_report.py b/src/src/robot_test/data_analysis/scripts/generate_report.py similarity index 100% rename from src/robot_test/data_analysis/scripts/generate_report.py rename to src/src/robot_test/data_analysis/scripts/generate_report.py diff --git a/src/src/robot_test/data_analysis/scripts/generate_report.py:Zone.Identifier b/src/src/robot_test/data_analysis/scripts/generate_report.py:Zone.Identifier new file mode 100644 index 0000000..582ba8c --- /dev/null +++ b/src/src/robot_test/data_analysis/scripts/generate_report.py:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\chloe\Downloads\src.zip diff --git a/src/robot_test/launch/standard_joint_test.launch.py b/src/src/robot_test/launch/standard_joint_test.launch.py similarity index 100% rename from src/robot_test/launch/standard_joint_test.launch.py rename to src/src/robot_test/launch/standard_joint_test.launch.py diff --git a/src/robot_test/package.xml b/src/src/robot_test/package.xml similarity index 100% rename from src/robot_test/package.xml rename to src/src/robot_test/package.xml diff --git a/src/robot_test/src/aruco_error_logger.cpp b/src/src/robot_test/src/aruco_error_logger.cpp similarity index 100% rename from src/robot_test/src/aruco_error_logger.cpp rename to src/src/robot_test/src/aruco_error_logger.cpp diff --git a/src/robot_test/src/standard_joint_test.cpp b/src/src/robot_test/src/standard_joint_test.cpp similarity index 100% rename from src/robot_test/src/standard_joint_test.cpp rename to src/src/robot_test/src/standard_joint_test.cpp