diff --git a/ariac_gazebo/config/sensors.yaml b/ariac_gazebo/config/sensors.yaml new file mode 100644 index 000000000..471438794 --- /dev/null +++ b/ariac_gazebo/config/sensors.yaml @@ -0,0 +1,15 @@ +robot_cameras: + floor_robot_camera: + active: false + type: rgb + + ceiling_robot_camera: + active: false + type: rgbd + +static_sensors: + conveyor_breakbeam: + type: break_beam + pose: + xyz: [-0.352, 3.91, 0.88] + rpy: [0, 0, pi] diff --git a/ariac_gazebo/config/trials/assembly.yaml b/ariac_gazebo/config/trials/assembly.yaml index effad9a55..488e63245 100644 --- a/ariac_gazebo/config/trials/assembly.yaml +++ b/ariac_gazebo/config/trials/assembly.yaml @@ -1,5 +1,5 @@ # Trial Name: assembly.yaml -# ARIAC2023 +# ARIAC2024 # ENVIRONMENT SETUP diff --git a/ariac_gazebo/config/trials/combined.yaml b/ariac_gazebo/config/trials/combined.yaml index f2055bc24..758a5baaf 100644 --- a/ariac_gazebo/config/trials/combined.yaml +++ b/ariac_gazebo/config/trials/combined.yaml @@ -9,6 +9,12 @@ kitting_trays: # Which kitting trays will be spawn tray_ids: [1] slots: [4] +assembly_inserts: + as1: '0.0' + as2: '0.0' + as3: '0.0' + as4: '0.0' + parts: bins: # bin params - 8 total bins each bin has nine total slots (1-9) bin1: diff --git a/ariac_gazebo/config/trials/kitting.yaml b/ariac_gazebo/config/trials/kitting.yaml index aa1efd973..6b065fd1f 100644 --- a/ariac_gazebo/config/trials/kitting.yaml +++ b/ariac_gazebo/config/trials/kitting.yaml @@ -1,5 +1,5 @@ -# Trial Name: qc.yaml -# ARIAC2023 +# Trial Name: kitting.yaml +# ARIAC2024 # ENVIRONMENT SETUP @@ -9,6 +9,12 @@ kitting_trays: # Which kitting trays will be spawn tray_ids: [3, 8] slots: [1, 4] +assembly_inserts: + as1: '0.0' + as2: '0.0' + as3: '0.0' + as4: '0.0' + parts: bins: # bin params - 8 total bins each bin has nine total slots (1-9) bin2: diff --git a/ariac_gazebo/config/trials/tutorial.yaml b/ariac_gazebo/config/trials/tutorial.yaml index 679e69052..3fc9fe8fb 100644 --- a/ariac_gazebo/config/trials/tutorial.yaml +++ b/ariac_gazebo/config/trials/tutorial.yaml @@ -1,5 +1,5 @@ # Trial Name: tutorial.yaml -# ARIAC2023 +# ARIAC2024 # ENVIRONMENT SETUP @@ -9,6 +9,12 @@ kitting_trays: # Which kitting trays will be spawn tray_ids: [3, 5, 8, 0] slots: [1, 3, 5, 6] +assembly_inserts: + as1: '0.0' + as2: '0.0' + as3: '0.0' + as4: '0.0' + parts: agvs: agv1: diff --git a/ariac_gazebo/launch/ariac.launch.py b/ariac_gazebo/launch/ariac.launch.py index ff09af3dc..3d35622c3 100644 --- a/ariac_gazebo/launch/ariac.launch.py +++ b/ariac_gazebo/launch/ariac.launch.py @@ -196,7 +196,7 @@ def generate_launch_description(): ) declared_arguments.append( - DeclareLaunchArgument("competitor_pkg", default_value="test_competitor", description="name of competitor package")) + DeclareLaunchArgument("competitor_pkg", default_value="ariac_gazebo", description="name of competitor package")) declared_arguments.append( DeclareLaunchArgument("sensor_config", default_value="sensors", description="name of user configuration file") diff --git a/test_competitor/CMakeLists.txt b/test_competitor/CMakeLists.txt deleted file mode 100644 index aff54842e..000000000 --- a/test_competitor/CMakeLists.txt +++ /dev/null @@ -1,90 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(test_competitor) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - moveit_ros_planning_interface - ariac_msgs - shape_msgs - tf2_kdl -) - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(${PROJECT_NAME} SHARED - src/test_competitor.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - include -) - -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Add main competitor executable -add_executable(competitor src/competitor.cpp) -target_include_directories(competitor PRIVATE include) -target_link_libraries(competitor - ${PROJECT_NAME} -) -ament_target_dependencies(competitor ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install(TARGETS competitor - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -# Add moveit test executable -add_executable(moveit_test src/moveit_test.cpp) -target_include_directories(moveit_test PRIVATE include) -target_link_libraries(moveit_test - ${PROJECT_NAME} -) -ament_target_dependencies(moveit_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install(TARGETS moveit_test - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install(DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY - config - launch - meshes - rviz - DESTINATION share/${PROJECT_NAME} -) - -ament_export_libraries( - ${PROJECT_NAME} -) - -ament_export_include_directories( - include -) - -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -ament_package() \ No newline at end of file diff --git a/test_competitor/config/sample.yaml b/test_competitor/config/sample.yaml deleted file mode 100644 index e19d6d467..000000000 --- a/test_competitor/config/sample.yaml +++ /dev/null @@ -1,58 +0,0 @@ -# Example sensor configuration with one of each sensor type -sensors: - breakbeam_0: - type: break_beam - visualize_fov: true - pose: - xyz: [-0.35, 3, 0.95] - rpy: [0, 0, pi] - - proximity_sensor_0: - type: proximity - visualize_fov: true - pose: - xyz: [-0.573, 2.84, 1] - rpy: [pi/2, pi/6, pi/2] - - laser_profiler_0: - type: laser_profiler - visualize_fov: true - pose: - xyz: [-0.573, 1.486, 1.526] - rpy: [pi/2, pi/2, 0] - - lidar_0: - type: lidar - visualize_fov: false - pose: - xyz: [-2.286, -2.96, 1.8] - rpy: [pi, pi/2, 0] - - rgb_camera_0: - type: rgb_camera - visualize_fov: false - pose: - xyz: [-2.286, 2.96, 1.8] - rpy: [pi, pi/2, 0] - - rgbd_camera_0: - type: rgbd_camera - visualize_fov: false - pose: - xyz: [-2.286, 4.96, 1.8] - rpy: [pi, pi/2, 0] - - basic_logical_camera_0: - visualize_fov: false - type: basic_logical_camera - pose: - xyz: [-2.286, 2.96, 1.8] - rpy: [pi, pi/2, 0] - - advanced_logical_camera_0: - visualize_fov: false - type: advanced_logical_camera - pose: - xyz: [-2.286, -2.96, 1.8] - rpy: [pi, pi/2, 0] - \ No newline at end of file diff --git a/test_competitor/config/sensors.yaml b/test_competitor/config/sensors.yaml deleted file mode 100644 index 885fdc262..000000000 --- a/test_competitor/config/sensors.yaml +++ /dev/null @@ -1,45 +0,0 @@ -robot_cameras: - floor_robot_camera: - active: true - type: rgb - - ceiling_robot_camera: - active: true - type: rgbd - -static_sensors: - right_bins_camera: - type: advanced_logical_camera - pose: - xyz: [-2.286, 2.96, 1.8] - rpy: [pi, pi/2, 0] - - left_bins_camera: - type: advanced_logical_camera - pose: - xyz: [-2.286, -2.96, 1.8] - rpy: [pi, pi/2, 0] - - kts1_camera: - type: advanced_logical_camera - pose: - xyz: [-1.3, -5.8, 1.8] - rpy: [pi, pi/2, pi/2] - - kts2_camera: - type: advanced_logical_camera - pose: - xyz: [-1.3, 5.8, 1.8] - rpy: [pi, pi/2, -pi/2] - - conveyor_breakbeam: - type: break_beam - pose: - xyz: [-0.352, 3.91, 0.88] - rpy: [0, 0, pi] - - conveyor_camera: - type: advanced_logical_camera - pose: - xyz: [-0.573, 3.915, 1.8] - rpy: [pi/2, pi/2, 0] diff --git a/test_competitor/include/test_competitor/test_competitor.hpp b/test_competitor/include/test_competitor/test_competitor.hpp deleted file mode 100644 index 7f87b7a66..000000000 --- a/test_competitor/include/test_competitor/test_competitor.hpp +++ /dev/null @@ -1,379 +0,0 @@ -#include -#include -#include - -#include - -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include "tf2/exceptions.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_ros/buffer.h" - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -class TestCompetitor : public rclcpp::Node -{ -public: - /// Constructor - TestCompetitor(); - - ~TestCompetitor(); - - void AddModelsToPlanningScene(); - - // Floor Robot Public Functions - void FloorRobotSendHome(); - bool FloorRobotSetGripperState(bool enable); - bool FloorRobotChangeGripper(std::string station, std::string gripper_type); - bool FloorRobotPickandPlaceTray(int tray_id, int agv_num); - bool FloorRobotPickBinPart(ariac_msgs::msg::Part part_to_pick); - bool FloorRobotPickConveyorPart(ariac_msgs::msg::Part part_to_pick); - bool FloorRobotPlacePartOnKitTray(int agv_num, int quadrant); - - // Ceiling Robot Public Functions - void CeilingRobotSendHome(); - bool CeilingRobotSetGripperState(bool enable); - - // ARIAC Functions - bool StartCompetition(); - bool EndCompetition(); - bool LockAGVTray(int agv_num); - bool UnlockAGVTray(int agv_num); - bool MoveAGV(int agv_num, int destination); - bool SubmitOrder(std::string order_id); - - bool CompleteOrders(); - bool CompleteKittingTask(ariac_msgs::msg::KittingTask task); - bool CompleteAssemblyTask(ariac_msgs::msg::AssemblyTask task); - bool CompleteCombinedTask(ariac_msgs::msg::CombinedTask task); - - // Mutex Lock for conveyor parts - std::mutex conveyor_parts_mutex; - -private: - // Robot Move Functions - bool FloorRobotMovetoTarget(); - bool FloorRobotMoveCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - std::pair FloorRobotPlanCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - void FloorRobotWaitForAttach(double timeout); - - bool CeilingRobotMovetoTarget(); - bool CeilingRobotMoveCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - std::pair CeilingRobotPlanCartesian(std::vector waypoints, double vsf, double asf, bool avoid_collisions); - void CeilingRobotWaitForAttach(double timeout); - bool CeilingRobotWaitForAssemble(int station, ariac_msgs::msg::AssemblyPart part); - bool CeilingRobotMoveToAssemblyStation(int station); - bool CeilingRobotPickAGVPart(ariac_msgs::msg::PartPose part); - bool CeilingRobotAssemblePart(int station, ariac_msgs::msg::AssemblyPart part); - - geometry_msgs::msg::Quaternion SetRobotOrientation(double rotation); - - // Helper Functions - void LogPose(geometry_msgs::msg::Pose p); - geometry_msgs::msg::Pose MultiplyPose(geometry_msgs::msg::Pose p1, geometry_msgs::msg::Pose p2); - geometry_msgs::msg::Pose BuildPose(double x, double y, double z, geometry_msgs::msg::Quaternion orientation); - geometry_msgs::msg::Pose FrameWorldPose(std::string frame_id); - double GetYaw(geometry_msgs::msg::Pose pose); - geometry_msgs::msg::Quaternion QuaternionFromRPY(double r, double p, double y); - - moveit_msgs::msg::CollisionObject CreateCollisionObject(std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose); - void AddModelToPlanningScene(std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose); - - // AGV location - std::map agv_locations_ = {{1, -1}, {2, -1}, {3, -1}, {4, -1}}; - - - // Callback Groups - rclcpp::CallbackGroup::SharedPtr client_cb_group_; - rclcpp::CallbackGroup::SharedPtr topic_cb_group_; - - // MoveIt Interfaces - moveit::planning_interface::MoveGroupInterface floor_robot_; - moveit::planning_interface::MoveGroupInterface ceiling_robot_; - - moveit::planning_interface::PlanningSceneInterface planning_scene_; - - trajectory_processing::TimeOptimalTrajectoryGeneration totg_; - - // TF - std::unique_ptr tf_buffer = std::make_unique(get_clock()); - std::shared_ptr tf_listener = std::make_shared(*tf_buffer); - - // Subscriptions - rclcpp::Subscription::SharedPtr orders_sub_; - rclcpp::Subscription::SharedPtr agv1_status_sub_; - rclcpp::Subscription::SharedPtr agv2_status_sub_; - rclcpp::Subscription::SharedPtr agv3_status_sub_; - rclcpp::Subscription::SharedPtr agv4_status_sub_; - rclcpp::Subscription::SharedPtr competition_state_sub_; - rclcpp::Subscription::SharedPtr kts1_camera_sub_; - rclcpp::Subscription::SharedPtr kts2_camera_sub_; - rclcpp::Subscription::SharedPtr left_bins_camera_sub_; - rclcpp::Subscription::SharedPtr right_bins_camera_sub_; - rclcpp::Subscription::SharedPtr conveyor_camera_sub_; - rclcpp::Subscription::SharedPtr breakbeam_sub_; - rclcpp::Subscription::SharedPtr conveyor_parts_sub_; - - rclcpp::Subscription::SharedPtr floor_gripper_state_sub_; - rclcpp::Subscription::SharedPtr ceiling_gripper_state_sub_; - - rclcpp::Subscription::SharedPtr as1_state_sub_; - rclcpp::Subscription::SharedPtr as2_state_sub_; - rclcpp::Subscription::SharedPtr as3_state_sub_; - rclcpp::Subscription::SharedPtr as4_state_sub_; - - // Assembly States - std::map assembly_station_states_; - - // Orders List - ariac_msgs::msg::Order current_order_; - std::vector orders_; - - unsigned int competition_state_; - - // Gripper State - ariac_msgs::msg::VacuumGripperState floor_gripper_state_; - ariac_msgs::msg::Part floor_robot_attached_part_; - ariac_msgs::msg::VacuumGripperState ceiling_gripper_state_; - ariac_msgs::msg::Part ceiling_robot_attached_part_; - - // Sensor poses - geometry_msgs::msg::Pose kts1_camera_pose_; - geometry_msgs::msg::Pose kts2_camera_pose_; - geometry_msgs::msg::Pose left_bins_camera_pose_; - geometry_msgs::msg::Pose right_bins_camera_pose_; - geometry_msgs::msg::Pose conveyor_camera_pose_; - geometry_msgs::msg::Pose breakbeam_pose_; - - // Trays - std::vector kts1_trays_; - std::vector kts2_trays_; - - // Bins - std::vector left_bins_parts_; - std::vector right_bins_parts_; - std::vector> conveyor_parts_; - std::vector conveyor_part_detected_; - std::vector conveyor_parts_expected_; - - // Sensor Callbacks - bool kts1_camera_recieved_data = false; - bool kts2_camera_recieved_data = false; - bool left_bins_camera_recieved_data = false; - bool right_bins_camera_recieved_data = false; - bool conveyor_camera_recieved_data = false; - bool breakbeam_received_data = false; - bool conveyor_parts_recieved_data = false; - - void kts1_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void kts2_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void left_bins_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void right_bins_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void conveyor_camera_cb(const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg); - void breakbeam_cb(const ariac_msgs::msg::BreakBeamStatus::ConstSharedPtr msg); - void conveyor_parts_cb(const ariac_msgs::msg::ConveyorParts::ConstSharedPtr msg); - - // Gripper State Callback - void floor_gripper_state_cb(const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg); - void ceiling_gripper_state_cb(const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg); - - void as1_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - void as2_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - void as3_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - void as4_state_cb(const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg); - - // AGV Status Callback - void agv1_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - void agv2_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - void agv3_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - void agv4_status_cb(const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg); - - // Orders Callback - void orders_cb(const ariac_msgs::msg::Order::ConstSharedPtr msg); - - // Competition state callback - void competition_state_cb(const ariac_msgs::msg::CompetitionState::ConstSharedPtr msg); - - // ARIAC Services - rclcpp::Client::SharedPtr quality_checker_; - rclcpp::Client::SharedPtr pre_assembly_poses_getter_; - rclcpp::Client::SharedPtr floor_robot_tool_changer_; - rclcpp::Client::SharedPtr floor_robot_gripper_enable_; - rclcpp::Client::SharedPtr ceiling_robot_gripper_enable_; - - // Breakbeam parameters - bool breakbeam_status = false; - float breakbeam_time_sec; - - // Constants - double conveyor_speed_ = 0.2; - double kit_tray_thickness_ = 0.01; - double drop_height_ = 0.005; - double pick_offset_ = 0.003; - double assembly_offset_ = 0.02; - double battery_grip_offset_ = -0.05; - - std::map part_types_ = { - {ariac_msgs::msg::Part::BATTERY, "battery"}, - {ariac_msgs::msg::Part::PUMP, "pump"}, - {ariac_msgs::msg::Part::REGULATOR, "regulator"}, - {ariac_msgs::msg::Part::SENSOR, "sensor"} - }; - - std::map part_colors_ = { - {ariac_msgs::msg::Part::RED, "red"}, - {ariac_msgs::msg::Part::BLUE, "blue"}, - {ariac_msgs::msg::Part::GREEN, "green"}, - {ariac_msgs::msg::Part::ORANGE, "orange"}, - {ariac_msgs::msg::Part::PURPLE, "purple"}, - }; - - // Part heights - std::map part_heights_ = { - {ariac_msgs::msg::Part::BATTERY, 0.04}, - {ariac_msgs::msg::Part::PUMP, 0.12}, - {ariac_msgs::msg::Part::REGULATOR, 0.07}, - {ariac_msgs::msg::Part::SENSOR, 0.07} - }; - - // Quadrant Offsets - std::map> quad_offsets_ = { - {ariac_msgs::msg::KittingPart::QUADRANT1, std::pair(-0.08, 0.12)}, - {ariac_msgs::msg::KittingPart::QUADRANT2, std::pair(0.08, 0.12)}, - {ariac_msgs::msg::KittingPart::QUADRANT3, std::pair(-0.08, -0.12)}, - {ariac_msgs::msg::KittingPart::QUADRANT4, std::pair(0.08, -0.12)}, - }; - - std::map rail_positions_ = { - {"agv1", -4.5}, - {"agv2", -1.2}, - {"agv3", 1.2}, - {"agv4", 4.5}, - {"left_bins", 3}, - {"right_bins", -3} - }; - - // Joint value targets for kitting stations - std::map floor_kts1_js_ = { - {"linear_actuator_joint", 4.0}, - {"floor_shoulder_pan_joint", 1.57}, - {"floor_shoulder_lift_joint", -1.57}, - {"floor_elbow_joint", 1.57}, - {"floor_wrist_1_joint", -1.57}, - {"floor_wrist_2_joint", -1.57}, - {"floor_wrist_3_joint", 0.0} - }; - - std::map floor_kts2_js_ = { - {"linear_actuator_joint", -4.0}, - {"floor_shoulder_pan_joint", -1.57}, - {"floor_shoulder_lift_joint", -1.57}, - {"floor_elbow_joint", 1.57}, - {"floor_wrist_1_joint", -1.57}, - {"floor_wrist_2_joint", -1.57}, - {"floor_wrist_3_joint", 0.0} - }; - - std::map ceiling_as1_js_ = { - {"gantry_x_axis_joint", 1}, - {"gantry_y_axis_joint", -3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map ceiling_as2_js_ = { - {"gantry_x_axis_joint", -4}, - {"gantry_y_axis_joint", -3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map ceiling_as3_js_ = { - {"gantry_x_axis_joint", 1}, - {"gantry_y_axis_joint", 3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map ceiling_as4_js_ = { - {"gantry_x_axis_joint", -4}, - {"gantry_y_axis_joint", 3}, - {"gantry_rotation_joint", 1.571}, - {"ceiling_shoulder_pan_joint", 0}, - {"ceiling_shoulder_lift_joint", -2.37}, - {"ceiling_elbow_joint", 2.37}, - {"ceiling_wrist_1_joint", 3.14}, - {"ceiling_wrist_2_joint", -1.57}, - {"ceiling_wrist_3_joint", 0} - }; - - std::map floor_conveyor_js_ = { - {"linear_actuator_joint", 0.0}, - {"floor_shoulder_pan_joint", 3.14}, - {"floor_shoulder_lift_joint", -0.9162979}, - {"floor_elbow_joint", 2.04204}, - {"floor_wrist_1_joint", -2.67035}, - {"floor_wrist_2_joint", -1.57}, - {"floor_wrist_3_joint", 0.0} - }; - - std::map agv_destination_ = { - {ariac_msgs::msg::AGVStatus::KITTING, "kitting"}, - {ariac_msgs::msg::AGVStatus::ASSEMBLY_FRONT, "assembly station front"}, - {ariac_msgs::msg::AGVStatus::ASSEMBLY_BACK, "assembly station back"}, - {ariac_msgs::msg::AGVStatus::WAREHOUSE, "warehouse"} - }; - - std::vector order_planning_scene_objects_; -}; diff --git a/test_competitor/launch/competitor.launch.py b/test_competitor/launch/competitor.launch.py deleted file mode 100644 index 4d013de2b..000000000 --- a/test_competitor/launch/competitor.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - OpaqueFunction, -) - -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration -from launch.conditions import IfCondition -from launch_ros.actions import Node - -from launch_ros.substitutions import FindPackageShare - -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder - - -def launch_setup(context, *args, **kwargs): - urdf = os.path.join(get_package_share_directory("ariac_description"), "urdf/ariac_robots/ariac_robots.urdf.xacro") - - moveit_config = ( - MoveItConfigsBuilder("ariac_robots", package_name="ariac_moveit_config") - .robot_description(urdf) - .robot_description_semantic(file_path="config/ariac_robots.srdf") - .trajectory_execution(file_path="config/moveit_controllers.yaml") - .planning_pipelines(pipelines=["ompl"]) - .to_moveit_configs() - ) - - test_competitor = Node( - package="test_competitor", - executable="competitor", - output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True}, - ], - arguments=['--ros-args', '--log-level', 'move_group_interface:=warn', '--log-level', 'moveit_trajectory_processing.time_optimal_trajectory_generation:=error'] - ) - - start_rviz = LaunchConfiguration("rviz") - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("test_competitor"), "rviz", "test_competitor.rviz"] - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2_moveit", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True} - ], - condition=IfCondition(start_rviz) - ) - - nodes_to_start = [ - test_competitor, - rviz_node - ] - - return nodes_to_start - -def generate_launch_description(): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument("rviz", default_value="false", description="start rviz node?") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/test_competitor/launch/moveit_test.launch.py b/test_competitor/launch/moveit_test.launch.py deleted file mode 100644 index 1f05bae7d..000000000 --- a/test_competitor/launch/moveit_test.launch.py +++ /dev/null @@ -1,81 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - OpaqueFunction, -) - -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration -from launch.conditions import IfCondition -from launch_ros.actions import Node - -from launch_ros.substitutions import FindPackageShare - -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder - - -def launch_setup(context, *args, **kwargs): - urdf = os.path.join(get_package_share_directory("ariac_description"), "urdf/ariac_robots/ariac_robots.urdf.xacro") - - moveit_config = ( - MoveItConfigsBuilder("ariac_robots", package_name="ariac_moveit_config") - .robot_description(urdf) - .robot_description_semantic(file_path="config/ariac_robots.srdf") - .trajectory_execution(file_path="config/moveit_controllers.yaml") - .planning_pipelines(pipelines=["ompl"]) - .to_moveit_configs() - ) - - moveit_test = Node( - package="test_competitor", - executable="moveit_test", - output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True}, - ], - arguments=['--ros-args', '--log-level', 'move_group_interface:=warn', '--log-level', 'moveit_trajectory_processing.time_optimal_trajectory_generation:=error'] - ) - - start_rviz = LaunchConfiguration("rviz") - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("test_competitor"), "rviz", "test_competitor.rviz"] - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2_moveit", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - {"use_sim_time": True} - ], - condition=IfCondition(start_rviz) - ) - - nodes_to_start = [ - moveit_test, - rviz_node - ] - - return nodes_to_start - -def generate_launch_description(): - declared_arguments = [] - - declared_arguments.append( - DeclareLaunchArgument("rviz", default_value="false", description="start rviz node?") - ) - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/test_competitor/meshes/assembly_insert.stl b/test_competitor/meshes/assembly_insert.stl deleted file mode 100644 index df6427de1..000000000 Binary files a/test_competitor/meshes/assembly_insert.stl and /dev/null differ diff --git a/test_competitor/meshes/assembly_station.stl b/test_competitor/meshes/assembly_station.stl deleted file mode 100644 index 48c05e674..000000000 Binary files a/test_competitor/meshes/assembly_station.stl and /dev/null differ diff --git a/test_competitor/meshes/battery.stl b/test_competitor/meshes/battery.stl deleted file mode 100644 index e36b9d480..000000000 Binary files a/test_competitor/meshes/battery.stl and /dev/null differ diff --git a/test_competitor/meshes/bin.stl b/test_competitor/meshes/bin.stl deleted file mode 100644 index 4394a86c3..000000000 Binary files a/test_competitor/meshes/bin.stl and /dev/null differ diff --git a/test_competitor/meshes/conveyor.stl b/test_competitor/meshes/conveyor.stl deleted file mode 100644 index 549fd5e0b..000000000 Binary files a/test_competitor/meshes/conveyor.stl and /dev/null differ diff --git a/test_competitor/meshes/kit_tray.stl b/test_competitor/meshes/kit_tray.stl deleted file mode 100644 index 6ccba497a..000000000 Binary files a/test_competitor/meshes/kit_tray.stl and /dev/null differ diff --git a/test_competitor/meshes/kit_tray_table.stl b/test_competitor/meshes/kit_tray_table.stl deleted file mode 100644 index 8c20cb9fe..000000000 Binary files a/test_competitor/meshes/kit_tray_table.stl and /dev/null differ diff --git a/test_competitor/meshes/pump.stl b/test_competitor/meshes/pump.stl deleted file mode 100644 index 192e56abe..000000000 Binary files a/test_competitor/meshes/pump.stl and /dev/null differ diff --git a/test_competitor/meshes/regulator.stl b/test_competitor/meshes/regulator.stl deleted file mode 100644 index 876f93384..000000000 Binary files a/test_competitor/meshes/regulator.stl and /dev/null differ diff --git a/test_competitor/meshes/sensor.stl b/test_competitor/meshes/sensor.stl deleted file mode 100644 index de6b1b7c3..000000000 Binary files a/test_competitor/meshes/sensor.stl and /dev/null differ diff --git a/test_competitor/package.xml b/test_competitor/package.xml deleted file mode 100644 index d8925e11d..000000000 --- a/test_competitor/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - test_competitor - 2024.1.0 - Example competitor package for the ARIAC simulation - justin - NIST - - ament_cmake - - rclcpp - std_msgs - ariac_msgs - shape_msgs - std_srvs - geometry_msgs - moveit_msgs - tf2_geometry_msgs - tf2_ros - orocos_kdl_vendor - - - ament_cmake - - diff --git a/test_competitor/rviz/test_competitor.rviz b/test_competitor/rviz/test_competitor.rviz deleted file mode 100644 index 099da382c..000000000 --- a/test_competitor/rviz/test_competitor.rviz +++ /dev/null @@ -1,654 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PlanningScene1 - - /PlanningScene1/Scene Geometry1 - - /PlanningScene1/Scene Robot1 - - /TF1 - - /TF1/Frames1 - Splitter Ratio: 0.5823529362678528 - Tree Height: 363 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: -8 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 16 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: /monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 206; 92; 0 - Scene Display Time: 9.999999747378752e-05 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 78; 154; 6 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - agv1_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv1_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv1_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv2_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv2_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv2_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv3_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv3_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv3_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv4_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - agv4_track: - Alpha: 1 - Show Axes: false - Show Trail: false - agv4_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_base: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_base_link_inertia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_ft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - ceiling_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ceiling_wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_base: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_base_link_inertia: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_flange: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_ft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - floor_upper_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - floor_wrist_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - long_rail_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - long_rail_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_base: - Alpha: 1 - Show Axes: false - Show Trail: false - slide_bar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - small_rail: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_main: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso_tray: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - agv1_base: - Value: false - agv1_track: - Value: false - agv1_tray: - Value: true - agv2_base: - Value: false - agv2_track: - Value: false - agv2_tray: - Value: true - agv3_base: - Value: false - agv3_track: - Value: false - agv3_tray: - Value: true - agv4_base: - Value: false - agv4_track: - Value: false - agv4_tray: - Value: true - as1_insert_frame: - Value: true - as1_table_frame: - Value: false - as2_insert_frame: - Value: true - as2_table_frame: - Value: false - as3_insert_frame: - Value: true - as3_table_frame: - Value: false - as4_insert_frame: - Value: true - as4_table_frame: - Value: false - bin1_frame: - Value: true - bin2_frame: - Value: true - bin3_frame: - Value: true - bin4_frame: - Value: true - bin5_frame: - Value: true - bin6_frame: - Value: true - bin7_frame: - Value: true - bin8_frame: - Value: true - ceiling_base: - Value: false - ceiling_base_link: - Value: false - ceiling_base_link_inertia: - Value: false - ceiling_flange: - Value: false - ceiling_forearm_link: - Value: false - ceiling_ft_frame: - Value: false - ceiling_gripper: - Value: true - ceiling_shoulder_link: - Value: false - ceiling_tool0: - Value: false - ceiling_upper_arm_link: - Value: false - ceiling_wrist_1_link: - Value: false - ceiling_wrist_2_link: - Value: false - ceiling_wrist_3_link: - Value: false - conveyor_belt_base_frame: - Value: false - conveyor_belt_part_spawn_frame: - Value: false - floor_base: - Value: false - floor_base_link: - Value: false - floor_base_link_inertia: - Value: false - floor_flange: - Value: false - floor_forearm_link: - Value: false - floor_ft_frame: - Value: false - floor_gripper: - Value: true - floor_shoulder_link: - Value: false - floor_tool0: - Value: false - floor_upper_arm_link: - Value: false - floor_wrist_1_link: - Value: false - floor_wrist_2_link: - Value: false - floor_wrist_3_link: - Value: false - kts1_camera_frame: - Value: false - kts1_table_frame: - Value: false - kts1_tool_changer_parts_frame: - Value: true - kts1_tool_changer_trays_frame: - Value: true - kts2_camera_frame: - Value: false - kts2_table_frame: - Value: false - kts2_tool_changer_parts_frame: - Value: true - kts2_tool_changer_trays_frame: - Value: true - left_bins_camera_frame: - Value: false - long_rail_1: - Value: false - long_rail_2: - Value: false - right_bins_camera_frame: - Value: false - robot_base: - Value: false - slide_bar: - Value: false - small_rail: - Value: false - torso_base: - Value: false - torso_main: - Value: false - torso_tray: - Value: false - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: false - Show Axes: true - Show Names: false - Tree: - world: - agv1_track: - agv1_base: - agv1_tray: - {} - agv2_track: - agv2_base: - agv2_tray: - {} - agv3_track: - agv3_base: - agv3_tray: - {} - agv4_track: - agv4_base: - agv4_tray: - {} - as1_table_frame: - as1_insert_frame: - {} - as2_table_frame: - as2_insert_frame: - {} - as3_table_frame: - as3_insert_frame: - {} - as4_table_frame: - as4_insert_frame: - {} - bin1_frame: - {} - bin2_frame: - {} - bin3_frame: - {} - bin4_frame: - {} - bin5_frame: - {} - bin6_frame: - {} - bin7_frame: - {} - bin8_frame: - {} - conveyor_belt_base_frame: - conveyor_belt_part_spawn_frame: - {} - kts1_camera_frame: - {} - kts1_table_frame: - kts1_tool_changer_parts_frame: - {} - kts1_tool_changer_trays_frame: - {} - kts2_camera_frame: - {} - kts2_table_frame: - kts2_tool_changer_parts_frame: - {} - kts2_tool_changer_trays_frame: - {} - left_bins_camera_frame: - {} - long_rail_1: - long_rail_2: - small_rail: - torso_base: - torso_main: - ceiling_base_link: - ceiling_base: - {} - ceiling_base_link_inertia: - ceiling_shoulder_link: - ceiling_upper_arm_link: - ceiling_forearm_link: - ceiling_wrist_1_link: - ceiling_wrist_2_link: - ceiling_wrist_3_link: - ceiling_flange: - ceiling_tool0: - {} - ceiling_ft_frame: - {} - ceiling_gripper: - {} - torso_tray: - {} - right_bins_camera_frame: - {} - slide_bar: - robot_base: - floor_base_link: - floor_base: - {} - floor_base_link_inertia: - floor_shoulder_link: - floor_upper_arm_link: - floor_forearm_link: - floor_wrist_1_link: - floor_wrist_2_link: - floor_wrist_3_link: - floor_flange: - floor_tool0: - {} - floor_ft_frame: - {} - floor_gripper: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 8.921540260314941 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -2.3044843673706055 - Y: -3.5008909702301025 - Z: 0.5595264434814453 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5853981971740723 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.978596210479736 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 716 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000176000001a90000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039c0000022e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 924 - X: 970 - Y: 57 diff --git a/test_competitor/src/competitor.cpp b/test_competitor/src/competitor.cpp deleted file mode 100644 index 8aeadfe24..000000000 --- a/test_competitor/src/competitor.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include - -#include - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - - auto test_competitor = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(test_competitor); - std::thread([&executor]() { executor.spin(); }).detach(); - - // Start Competition - test_competitor->StartCompetition(); - - test_competitor->AddModelsToPlanningScene(); - - sleep(2); - - // Move Robots to Home Poses - test_competitor->FloorRobotSendHome(); - test_competitor->CeilingRobotSendHome(); - - // Complete Orders - test_competitor->CompleteOrders(); - - test_competitor->EndCompetition(); - - rclcpp::shutdown(); -} \ No newline at end of file diff --git a/test_competitor/src/moveit_test.cpp b/test_competitor/src/moveit_test.cpp deleted file mode 100644 index 42395a6da..000000000 --- a/test_competitor/src/moveit_test.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - auto test_competitor = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(test_competitor); - std::thread([&executor]() { executor.spin(); }).detach(); - - // Start Competition - test_competitor->StartCompetition(); - - // Send Robots to home position - test_competitor->FloorRobotSendHome(); - test_competitor->CeilingRobotSendHome(); - - // Shutdown ROS - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/test_competitor/src/test_competitor.cpp b/test_competitor/src/test_competitor.cpp deleted file mode 100644 index c14939824..000000000 --- a/test_competitor/src/test_competitor.cpp +++ /dev/null @@ -1,1984 +0,0 @@ -#include - -TestCompetitor::TestCompetitor() - : Node("test_competitor"), - floor_robot_(std::shared_ptr(std::move(this)), "floor_robot"), - ceiling_robot_(std::shared_ptr(std::move(this)), "ceiling_robot"), - planning_scene_() -{ - // Use upper joint velocity and acceleration limits - floor_robot_.setMaxAccelerationScalingFactor(1.0); - floor_robot_.setMaxVelocityScalingFactor(1.0); - floor_robot_.setPlanningTime(10.0); - floor_robot_.setNumPlanningAttempts(5); - floor_robot_.allowReplanning(true); - floor_robot_.setReplanAttempts(5); - - ceiling_robot_.setMaxAccelerationScalingFactor(1.0); - ceiling_robot_.setMaxVelocityScalingFactor(1.0); - ceiling_robot_.setPlanningTime(10.0); - ceiling_robot_.setNumPlanningAttempts(5); - ceiling_robot_.allowReplanning(true); - ceiling_robot_.setReplanAttempts(5); - // Subscribe to topics - rclcpp::SubscriptionOptions options; - - topic_cb_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - options.callback_group = topic_cb_group_; - - orders_sub_ = this->create_subscription("/ariac/orders", 1, - std::bind(&TestCompetitor::orders_cb, this, std::placeholders::_1), options); - - competition_state_sub_ = this->create_subscription("/ariac/competition_state", 1, - std::bind(&TestCompetitor::competition_state_cb, this, std::placeholders::_1), options); - - kts1_camera_sub_ = this->create_subscription( - "/ariac/sensors/kts1_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::kts1_camera_cb, this, std::placeholders::_1), options); - - kts2_camera_sub_ = this->create_subscription( - "/ariac/sensors/kts2_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::kts2_camera_cb, this, std::placeholders::_1), options); - - left_bins_camera_sub_ = this->create_subscription( - "/ariac/sensors/left_bins_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::left_bins_camera_cb, this, std::placeholders::_1), options); - - right_bins_camera_sub_ = this->create_subscription( - "/ariac/sensors/right_bins_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::right_bins_camera_cb, this, std::placeholders::_1), options); - - conveyor_camera_sub_ = this->create_subscription( - "/ariac/sensors/conveyor_camera/image", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::conveyor_camera_cb, this, std::placeholders::_1), options); - - breakbeam_sub_ = this->create_subscription( - "/ariac/sensors/conveyor_breakbeam/change", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::breakbeam_cb, this, std::placeholders::_1), options); - - conveyor_parts_sub_ = this->create_subscription( - "/ariac/conveyor_parts", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::conveyor_parts_cb, this, std::placeholders::_1), options); - - floor_gripper_state_sub_ = this->create_subscription( - "/ariac/floor_robot_gripper_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::floor_gripper_state_cb, this, std::placeholders::_1), options); - - ceiling_gripper_state_sub_ = this->create_subscription( - "/ariac/ceiling_robot_gripper_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::ceiling_gripper_state_cb, this, std::placeholders::_1), options); - - as1_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_1_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as1_state_cb, this, std::placeholders::_1), options); - - as2_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_2_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as2_state_cb, this, std::placeholders::_1), options); - - as3_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_3_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as3_state_cb, this, std::placeholders::_1), options); - - as4_state_sub_ = this->create_subscription( - "/ariac/assembly_insert_4_assembly_state", rclcpp::SensorDataQoS(), - std::bind(&TestCompetitor::as4_state_cb, this, std::placeholders::_1), options); - - agv1_status_sub_ = this->create_subscription( - "/ariac/agv1_status", 10, - std::bind(&TestCompetitor::agv1_status_cb, this, std::placeholders::_1), options); - - agv2_status_sub_ = this->create_subscription( - "/ariac/agv2_status", 10, - std::bind(&TestCompetitor::agv2_status_cb, this, std::placeholders::_1), options); - - agv3_status_sub_ = this->create_subscription( - "/ariac/agv3_status", 10, - std::bind(&TestCompetitor::agv3_status_cb, this, std::placeholders::_1), options); - - agv4_status_sub_ = this->create_subscription( - "/ariac/agv4_status", 10, - std::bind(&TestCompetitor::agv4_status_cb, this, std::placeholders::_1), options); - - // Initialize service clients - quality_checker_ = this->create_client("/ariac/perform_quality_check"); - pre_assembly_poses_getter_ = this->create_client("/ariac/get_pre_assembly_poses"); - floor_robot_tool_changer_ = this->create_client("/ariac/floor_robot_change_gripper"); - floor_robot_gripper_enable_ = this->create_client("/ariac/floor_robot_enable_gripper"); - ceiling_robot_gripper_enable_ = this->create_client("/ariac/ceiling_robot_enable_gripper"); - - RCLCPP_INFO(this->get_logger(), "Initialization successful."); -} - -TestCompetitor::~TestCompetitor() -{ - floor_robot_.~MoveGroupInterface(); - ceiling_robot_.~MoveGroupInterface(); -} - -void TestCompetitor::orders_cb( - const ariac_msgs::msg::Order::ConstSharedPtr msg) -{ - orders_.push_back(*msg); -} - -void TestCompetitor::competition_state_cb( - const ariac_msgs::msg::CompetitionState::ConstSharedPtr msg) -{ - competition_state_ = msg->competition_state; -} - -void TestCompetitor::kts1_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!kts1_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from kts1 camera"); - kts1_camera_recieved_data = true; - } - - kts1_trays_ = msg->tray_poses; - kts1_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::kts2_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!kts2_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from kts2 camera"); - kts2_camera_recieved_data = true; - } - - kts2_trays_ = msg->tray_poses; - kts2_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::left_bins_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!left_bins_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from left bins camera"); - left_bins_camera_recieved_data = true; - } - - left_bins_parts_ = msg->part_poses; - left_bins_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::right_bins_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!right_bins_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from right bins camera"); - right_bins_camera_recieved_data = true; - } - - right_bins_parts_ = msg->part_poses; - right_bins_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::conveyor_parts_cb( - const ariac_msgs::msg::ConveyorParts::ConstSharedPtr msg -) -{ - if (!conveyor_parts_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from conveyor parts"); - conveyor_parts_recieved_data = true; - } - - conveyor_parts_expected_ = msg->parts; -} - -void TestCompetitor::conveyor_camera_cb( - const ariac_msgs::msg::AdvancedLogicalCameraImage::ConstSharedPtr msg) -{ - if (!conveyor_camera_recieved_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from conveyor camera"); - conveyor_camera_recieved_data = true; - } - - conveyor_part_detected_ = msg->part_poses; - conveyor_camera_pose_ = msg->sensor_pose; -} - -void TestCompetitor::breakbeam_cb( - const ariac_msgs::msg::BreakBeamStatus::ConstSharedPtr msg) -{ - if (!breakbeam_received_data) - { - RCLCPP_DEBUG(get_logger(), "Received data from conveyor breakbeam"); - breakbeam_received_data = true; - breakbeam_pose_ = FrameWorldPose(msg->header.frame_id); - } - - breakbeam_status = msg->object_detected; - ariac_msgs::msg::PartPose part_to_add; - auto detection_time = now(); - float prev_distance = 0; - int count = 0; - - if (breakbeam_status) - { - // Lock conveyor_parts_ mutex - std::lock_guard lock(conveyor_parts_mutex); - - for (auto part : conveyor_part_detected_) - { - geometry_msgs::msg::Pose part_pose = MultiplyPose(conveyor_camera_pose_, part.pose); - float distance = abs(part_pose.position.y - breakbeam_pose_.position.y); - if (count == 0) - { - part_to_add = part; - detection_time = now(); - prev_distance = distance; - count++; - } - else - { - if (distance < prev_distance) - { - part_to_add = part; - detection_time = now(); - prev_distance = distance; - } - } - } - conveyor_parts_.emplace_back(part_to_add, detection_time); - } -} - -void TestCompetitor::floor_gripper_state_cb( - const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg) -{ - floor_gripper_state_ = *msg; -} - -void TestCompetitor::ceiling_gripper_state_cb( - const ariac_msgs::msg::VacuumGripperState::ConstSharedPtr msg) -{ - ceiling_gripper_state_ = *msg; -} - -void TestCompetitor::as1_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS1, *msg); -} - -void TestCompetitor::as2_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS2, *msg); -} - -void TestCompetitor::as3_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS3, *msg); -} - -void TestCompetitor::as4_state_cb( - const ariac_msgs::msg::AssemblyState::ConstSharedPtr msg) -{ - assembly_station_states_.insert_or_assign(ariac_msgs::msg::AssemblyTask::AS4, *msg); -} - -void TestCompetitor::agv1_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[1] = msg->location; -} - -void TestCompetitor::agv2_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[2] = msg->location; -} - -void TestCompetitor::agv3_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[3] = msg->location; -} - -void TestCompetitor::agv4_status_cb( - const ariac_msgs::msg::AGVStatus::ConstSharedPtr msg) -{ - agv_locations_[4] = msg->location; -} - -geometry_msgs::msg::Pose TestCompetitor::MultiplyPose( - geometry_msgs::msg::Pose p1, geometry_msgs::msg::Pose p2) -{ - KDL::Frame f1; - KDL::Frame f2; - - tf2::fromMsg(p1, f1); - tf2::fromMsg(p2, f2); - - KDL::Frame f3 = f1 * f2; - - return tf2::toMsg(f3); -} - -void TestCompetitor::LogPose(geometry_msgs::msg::Pose p) -{ - tf2::Quaternion q( - p.orientation.x, - p.orientation.y, - p.orientation.z, - p.orientation.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - roll *= 180 / M_PI; - pitch *= 180 / M_PI; - yaw *= 180 / M_PI; - - RCLCPP_INFO(get_logger(), "(X: %.2f, Y: %.2f, Z: %.2f, R: %.2f, P: %.2f, Y: %.2f)", - p.position.x, p.position.y, p.position.z, - roll, pitch, yaw); -} - -geometry_msgs::msg::Pose TestCompetitor::BuildPose( - double x, double y, double z, geometry_msgs::msg::Quaternion orientation) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; - pose.orientation = orientation; - - return pose; -} - -geometry_msgs::msg::Pose TestCompetitor::FrameWorldPose(std::string frame_id) -{ - geometry_msgs::msg::TransformStamped t; - geometry_msgs::msg::Pose pose; - - try - { - t = tf_buffer->lookupTransform("world", frame_id, tf2::TimePointZero); - } - catch (const tf2::TransformException &ex) - { - RCLCPP_ERROR(get_logger(), "Could not get transform"); - } - - pose.position.x = t.transform.translation.x; - pose.position.y = t.transform.translation.y; - pose.position.z = t.transform.translation.z; - pose.orientation = t.transform.rotation; - - return pose; -} - -double TestCompetitor::GetYaw(geometry_msgs::msg::Pose pose) -{ - tf2::Quaternion q( - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - return yaw; -} - -geometry_msgs::msg::Quaternion TestCompetitor::QuaternionFromRPY(double r, double p, double y) -{ - tf2::Quaternion q; - geometry_msgs::msg::Quaternion q_msg; - - q.setRPY(r, p, y); - - q_msg.x = q.x(); - q_msg.y = q.y(); - q_msg.z = q.z(); - q_msg.w = q.w(); - - return q_msg; -} - -moveit_msgs::msg::CollisionObject TestCompetitor::CreateCollisionObject( - std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose) -{ - moveit_msgs::msg::CollisionObject collision; - - collision.id = name; - collision.header.frame_id = "world"; - - shape_msgs::msg::Mesh mesh; - shapes::ShapeMsg mesh_msg; - - std::string package_share_directory = ament_index_cpp::get_package_share_directory("test_competitor"); - std::stringstream path; - path << "file://" << package_share_directory << "/meshes/" << mesh_file; - std::string model_path = path.str(); - - shapes::Mesh *m = shapes::createMeshFromResource(model_path); - shapes::constructMsgFromShape(m, mesh_msg); - - mesh = boost::get(mesh_msg); - - collision.meshes.push_back(mesh); - collision.mesh_poses.push_back(model_pose); - - collision.operation = collision.ADD; - - return collision; -} - -void TestCompetitor::AddModelToPlanningScene( - std::string name, std::string mesh_file, geometry_msgs::msg::Pose model_pose) -{ - planning_scene_.applyCollisionObject(CreateCollisionObject(name, mesh_file, model_pose)); -} - -void TestCompetitor::AddModelsToPlanningScene() -{ - std::vector objects; - - // Add bins - std::map> bin_positions = { - {"bin1", std::pair(-1.9, 3.375)}, - {"bin2", std::pair(-1.9, 2.625)}, - {"bin3", std::pair(-2.65, 2.625)}, - {"bin4", std::pair(-2.65, 3.375)}, - {"bin5", std::pair(-1.9, -3.375)}, - {"bin6", std::pair(-1.9, -2.625)}, - {"bin7", std::pair(-2.65, -2.625)}, - {"bin8", std::pair(-2.65, -3.375)}}; - - geometry_msgs::msg::Pose bin_pose; - for (auto const &bin : bin_positions) - { - bin_pose.position.x = bin.second.first; - bin_pose.position.y = bin.second.second; - bin_pose.position.z = 0; - bin_pose.orientation = QuaternionFromRPY(0, 0, 3.14159); - - objects.push_back(CreateCollisionObject(bin.first, "bin.stl", bin_pose)); - } - - // Add assembly stations - std::map> assembly_station_positions = { - {"as1", std::pair(-7.3, 3)}, - {"as2", std::pair(-12.3, 3)}, - {"as3", std::pair(-7.3, -3)}, - {"as4", std::pair(-12.3, -3)}, - }; - - geometry_msgs::msg::Pose assembly_station_pose; - for (auto const &station : assembly_station_positions) - { - assembly_station_pose.position.x = station.second.first; - assembly_station_pose.position.y = station.second.second; - assembly_station_pose.position.z = 0; - assembly_station_pose.orientation = QuaternionFromRPY(0, 0, 0); - - objects.push_back(CreateCollisionObject(station.first, "assembly_station.stl", assembly_station_pose)); - } - - // Add assembly briefcases - std::map> assembly_insert_positions = { - {"as1_insert", std::pair(-7.7, 3)}, - {"as2_insert", std::pair(-12.7, 3)}, - {"as3_insert", std::pair(-7.7, -3)}, - {"as4_insert", std::pair(-12.7, -3)}, - }; - - geometry_msgs::msg::Pose assembly_insert_pose; - - for (auto const &insert : assembly_insert_positions) - { - // assembly_insert_pose.position.x = insert.second.first; - // assembly_insert_pose.position.y = insert.second.second; - // assembly_insert_pose.position.z = 1.011; - // assembly_insert_pose.orientation = QuaternionFromRPY(0, 0, 0); - - std::string frame_name = insert.first + "_frame"; - - objects.push_back(CreateCollisionObject(insert.first, "assembly_insert.stl", FrameWorldPose(frame_name))); - } - - geometry_msgs::msg::Pose conveyor_pose; - conveyor_pose.position.x = -0.6; - conveyor_pose.position.y = 0; - conveyor_pose.position.z = 0; - conveyor_pose.orientation = QuaternionFromRPY(0, 0, 0); - - objects.push_back(CreateCollisionObject("conveyor", "conveyor.stl", conveyor_pose)); - - geometry_msgs::msg::Pose kts1_table_pose; - kts1_table_pose.position.x = -1.3; - kts1_table_pose.position.y = -5.84; - kts1_table_pose.position.z = 0; - kts1_table_pose.orientation = QuaternionFromRPY(0, 0, 3.14159); - - objects.push_back(CreateCollisionObject("kts1_table", "kit_tray_table.stl", kts1_table_pose)); - - geometry_msgs::msg::Pose kts2_table_pose; - kts2_table_pose.position.x = -1.3; - kts2_table_pose.position.y = 5.84; - kts2_table_pose.position.z = 0; - kts2_table_pose.orientation = QuaternionFromRPY(0, 0, 0); - - objects.push_back(CreateCollisionObject("kts2_table", "kit_tray_table.stl", kts2_table_pose)); - - if (!planning_scene_.applyCollisionObjects(objects)) { - RCLCPP_WARN(get_logger(), "Unable to add objects to planning scene"); - } -} - -geometry_msgs::msg::Quaternion TestCompetitor::SetRobotOrientation(double rotation) -{ - tf2::Quaternion tf_q; - tf_q.setRPY(0, 3.14159, rotation); - - geometry_msgs::msg::Quaternion q; - - q.x = tf_q.x(); - q.y = tf_q.y(); - q.z = tf_q.z(); - q.w = tf_q.w(); - - return q; -} - -bool TestCompetitor::FloorRobotMovetoTarget() -{ - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = static_cast(floor_robot_.plan(plan)); - - if (success) - { - return static_cast(floor_robot_.execute(plan)); - } - else - { - RCLCPP_ERROR(get_logger(), "Unable to generate plan"); - return false; - } -} - -bool TestCompetitor::FloorRobotMoveCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = floor_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return false; - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(floor_robot_.getCurrentState()->getRobotModel(), "floor_robot"); - rt.setRobotTrajectoryMsg(*floor_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return static_cast(floor_robot_.execute(trajectory)); -} - -std::pair TestCompetitor::FloorRobotPlanCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = floor_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return std::make_pair(false, trajectory); - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(floor_robot_.getCurrentState()->getRobotModel(), "floor_robot"); - rt.setRobotTrajectoryMsg(*floor_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return std::make_pair(true, trajectory); -} - -void TestCompetitor::FloorRobotWaitForAttach(double timeout) -{ - // Wait for part to be attached - rclcpp::Time start = now(); - std::vector waypoints; - geometry_msgs::msg::Pose starting_pose = floor_robot_.getCurrentPose().pose; - - while (!floor_gripper_state_.attached) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for gripper attach"); - - waypoints.clear(); - starting_pose.position.z -= 0.001; - waypoints.push_back(starting_pose); - - FloorRobotMoveCartesian(waypoints, 0.01, 0.01, true); - - usleep(500); - - if (now() - start > rclcpp::Duration::from_seconds(timeout)) - { - RCLCPP_ERROR(get_logger(), "Unable to pick up object"); - return; - } - } -} - -void TestCompetitor::FloorRobotSendHome() -{ - // Move floor robot to home joint state - RCLCPP_INFO_STREAM(get_logger(), "Moving Floor Robot to home position"); - floor_robot_.setNamedTarget("home"); - FloorRobotMovetoTarget(); -} - -bool TestCompetitor::FloorRobotSetGripperState(bool enable) -{ - if (floor_gripper_state_.enabled == enable) - { - if (floor_gripper_state_.enabled) - RCLCPP_INFO(get_logger(), "Already enabled"); - else - RCLCPP_INFO(get_logger(), "Already disabled"); - - return false; - } - - // Call enable service - auto request = std::make_shared(); - request->enable = enable; - - auto future = floor_robot_gripper_enable_->async_send_request(request); - future.wait(); - - if (!future.get()->success) - { - RCLCPP_ERROR(get_logger(), "Error calling gripper enable service"); - return false; - } - - return true; -} - -bool TestCompetitor::FloorRobotChangeGripper(std::string station, std::string gripper_type) -{ - // Move gripper into tool changer - RCLCPP_INFO_STREAM(get_logger(), "Changing gripper to " << gripper_type << " gripper"); - - auto tc_pose = FrameWorldPose(station + "_tool_changer_" + gripper_type + "_frame"); - - std::vector waypoints; - waypoints.push_back(BuildPose(tc_pose.position.x, tc_pose.position.y, - tc_pose.position.z + 0.4, SetRobotOrientation(0.0))); - - waypoints.push_back(BuildPose(tc_pose.position.x, tc_pose.position.y, - tc_pose.position.z, SetRobotOrientation(0.0))); - - if (!FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true)) - return false; - - // Call service to change gripper - auto request = std::make_shared(); - - if (gripper_type == "trays") - { - request->gripper_type = ariac_msgs::srv::ChangeGripper::Request::TRAY_GRIPPER; - } - else if (gripper_type == "parts") - { - request->gripper_type = ariac_msgs::srv::ChangeGripper::Request::PART_GRIPPER; - } - - auto future = floor_robot_tool_changer_->async_send_request(request); - future.wait(); - if (!future.get()->success) - { - RCLCPP_ERROR(get_logger(), "Error calling gripper change service"); - return false; - } - - waypoints.clear(); - waypoints.push_back(BuildPose(tc_pose.position.x, tc_pose.position.y, - tc_pose.position.z + 0.4, SetRobotOrientation(0.0))); - - if (!FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true)) - return false; - - return true; -} - -bool TestCompetitor::FloorRobotPickandPlaceTray(int tray_id, int agv_num) -{ - RCLCPP_INFO_STREAM(get_logger(), "Attempting to pick up kit tray " << tray_id << " and place on AGV " << agv_num); - - // Check if kit tray is on one of the two tables - geometry_msgs::msg::Pose tray_pose; - std::string station; - bool found_tray = false; - - // Check table 1 - for (auto tray : kts1_trays_) - { - if (tray.id == tray_id) - { - station = "kts1"; - tray_pose = MultiplyPose(kts1_camera_pose_, tray.pose); - found_tray = true; - break; - } - } - // Check table 2 - if (!found_tray) - { - for (auto tray : kts2_trays_) - { - if (tray.id == tray_id) - { - station = "kts2"; - tray_pose = MultiplyPose(kts2_camera_pose_, tray.pose); - found_tray = true; - break; - } - } - } - if (!found_tray) - return false; - - double tray_rotation = GetYaw(tray_pose); - - RCLCPP_INFO_STREAM(get_logger(), "Found kit tray " << tray_id << " on ktting tray station " << station << " moving to pick location"); - - // Move floor robot to the corresponding kit tray table - if (station == "kts1") - { - floor_robot_.setJointValueTarget(floor_kts1_js_); - } - else - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - } - FloorRobotMovetoTarget(); - - // Change gripper to tray gripper - if (floor_gripper_state_.type != "tray_gripper") - { - FloorRobotChangeGripper(station, "trays"); - } - - // Move to tray - std::vector waypoints; - - waypoints.push_back(BuildPose(tray_pose.position.x, tray_pose.position.y, - tray_pose.position.z + 0.2, SetRobotOrientation(tray_rotation))); - waypoints.push_back(BuildPose(tray_pose.position.x, tray_pose.position.y, - tray_pose.position.z + pick_offset_, SetRobotOrientation(tray_rotation))); - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - FloorRobotSetGripperState(true); - - FloorRobotWaitForAttach(3.0); - - RCLCPP_INFO_STREAM(get_logger(), "Picked kit tray " << tray_id); - - // Add kit tray to planning scene - std::string tray_name = "kit_tray_" + std::to_string(tray_id); - AddModelToPlanningScene(tray_name, "kit_tray.stl", tray_pose); - floor_robot_.attachObject(tray_name); - - order_planning_scene_objects_.push_back(tray_name); - - // Move up slightly - waypoints.clear(); - - waypoints.push_back(BuildPose(tray_pose.position.x, tray_pose.position.y, - tray_pose.position.z + 0.2, SetRobotOrientation(tray_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.05, 0.05, true); - - if (station == "kts1") - { - floor_robot_.setJointValueTarget(floor_kts1_js_); - } - else - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - } - FloorRobotMovetoTarget(); - - RCLCPP_INFO_STREAM(get_logger(), "Moving tray to AGV " << agv_num); - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_["agv" + std::to_string(agv_num)]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - - FloorRobotMovetoTarget(); - - auto agv_tray_pose = FrameWorldPose("agv" + std::to_string(agv_num) + "_tray"); - auto agv_rotation = GetYaw(agv_tray_pose); - - waypoints.clear(); - waypoints.push_back(BuildPose(agv_tray_pose.position.x, agv_tray_pose.position.y, - agv_tray_pose.position.z + 0.3, SetRobotOrientation(agv_rotation))); - - waypoints.push_back(BuildPose(agv_tray_pose.position.x, agv_tray_pose.position.y, - agv_tray_pose.position.z + kit_tray_thickness_ + drop_height_, SetRobotOrientation(agv_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true); - - FloorRobotSetGripperState(false); - - floor_robot_.detachObject(tray_name); - - LockAGVTray(agv_num); - - waypoints.clear(); - waypoints.push_back(BuildPose(agv_tray_pose.position.x, agv_tray_pose.position.y, - agv_tray_pose.position.z + 0.3, SetRobotOrientation(agv_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, false); - - return true; -} - -bool TestCompetitor::FloorRobotPickBinPart(ariac_msgs::msg::Part part_to_pick) -{ - RCLCPP_INFO_STREAM(get_logger(), "Attempting to pick a " << part_colors_[part_to_pick.color] << " " << part_types_[part_to_pick.type] << " from the bins"); - - // Check if part is in one of the bins - geometry_msgs::msg::Pose part_pose; - bool found_part = false; - std::string bin_side; - - // Check left bins - for (auto part : left_bins_parts_) - { - if (part.part.type == part_to_pick.type && part.part.color == part_to_pick.color) - { - part_pose = MultiplyPose(left_bins_camera_pose_, part.pose); - found_part = true; - bin_side = "left_bins"; - break; - } - } - // Check right bins - if (!found_part) - { - for (auto part : right_bins_parts_) - { - if (part.part.type == part_to_pick.type && part.part.color == part_to_pick.color) - { - part_pose = MultiplyPose(right_bins_camera_pose_, part.pose); - found_part = true; - bin_side = "right_bins"; - break; - } - } - } - if (!found_part) - { - RCLCPP_INFO(get_logger(), "Unable to locate part in the bins"); - return false; - } - - RCLCPP_INFO_STREAM(get_logger(), "Found part in " << bin_side << " checking gripper type and moving to pick location"); - - double part_rotation = GetYaw(part_pose); - - // Change gripper at location closest to part - if (floor_gripper_state_.type != "part_gripper") - { - std::string station; - if (part_pose.position.y < 0) - { - station = "kts1"; - } - else - { - station = "kts2"; - } - - // Move floor robot to the corresponding kit tray table - if (station == "kts1") - { - floor_robot_.setJointValueTarget(floor_kts1_js_); - } - else - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - } - FloorRobotMovetoTarget(); - - FloorRobotChangeGripper(station, "parts"); - } - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_[bin_side]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - std::vector waypoints; - waypoints.push_back(BuildPose(part_pose.position.x, part_pose.position.y, - part_pose.position.z + 0.5, SetRobotOrientation(part_rotation))); - - waypoints.push_back(BuildPose(part_pose.position.x, part_pose.position.y, - part_pose.position.z + part_heights_[part_to_pick.type] + pick_offset_, SetRobotOrientation(part_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - FloorRobotSetGripperState(true); - - FloorRobotWaitForAttach(3.0); - - RCLCPP_INFO_STREAM(get_logger(), "Picked Up the " << part_colors_[part_to_pick.color] << " " << part_types_[part_to_pick.type] << " from the bins"); - - // Add part to planning scene - std::string part_name = part_colors_[part_to_pick.color] + "_" + part_types_[part_to_pick.type]; - AddModelToPlanningScene(part_name, part_types_[part_to_pick.type] + ".stl", part_pose); - floor_robot_.attachObject(part_name); - floor_robot_attached_part_ = part_to_pick; - - order_planning_scene_objects_.push_back(part_name); - - // Move up slightly - waypoints.clear(); - waypoints.push_back(BuildPose(part_pose.position.x, part_pose.position.y, - part_pose.position.z + 0.2, SetRobotOrientation(part_rotation))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, true); - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_[bin_side]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - return true; -} - -bool TestCompetitor::FloorRobotPickConveyorPart(ariac_msgs::msg::Part part_to_pick) -{ - if (conveyor_parts_expected_.empty()) - { - RCLCPP_INFO(get_logger(), "No parts expected on the conveyor"); - return false; - } - for (auto parts : conveyor_parts_expected_){ - if (parts.part.type == part_to_pick.type && parts.part.color == part_to_pick.color){ - RCLCPP_INFO_STREAM(get_logger(), "Attempting to pick a " << part_colors_[part_to_pick.color] << " " << part_types_[part_to_pick.type] << " from the conveyor"); - break; - } - else if (parts == conveyor_parts_expected_.back()){ - RCLCPP_INFO(get_logger(), "Unable to locate part on the conveyor"); - return false; - } - } - - bool found_part = false; - bool part_picked = false; - int num_tries = 0; - geometry_msgs::msg::Pose part_pose; - rclcpp::Duration elapsed_time(0, 0); - builtin_interfaces::msg::Duration time_to_pick; - rclcpp::Time detection_time; - - // Change gripper at Kitting Tray Station 2 - if (floor_gripper_state_.type != "part_gripper") - { - floor_robot_.setJointValueTarget(floor_kts2_js_); - FloorRobotMovetoTarget(); - FloorRobotChangeGripper("kts2", "parts"); - } - RCLCPP_INFO_STREAM(get_logger(), "Moving Floor Robot to Conveyor pick location"); - while(!part_picked && num_tries < 3){ - // Move robot to predefined pick location - floor_robot_.setJointValueTarget(floor_conveyor_js_); - FloorRobotMovetoTarget(); - - // Find the requested part on the conveyor - do { - { - // Lock conveyor_parts_ mutex - std::lock_guard lock(conveyor_parts_mutex); - auto it = conveyor_parts_.begin(); - for (; it != conveyor_parts_.end(); ) { - auto part = it->first.part; - if (part.type == part_to_pick.type && part.color == part_to_pick.color) - { - part_pose = MultiplyPose(conveyor_camera_pose_, it->first.pose); - detection_time = it->second; - - elapsed_time = rclcpp::Time(now()) - detection_time; - auto current_part_position_ = part_pose.position.y - (elapsed_time.seconds() * conveyor_speed_); - // Check if part hasn't passed the pick location - if (current_part_position_ > 0) - { - time_to_pick.sec = current_part_position_ / conveyor_speed_; - // Check if part has more than 5 seconds to arrive at pick location - if (time_to_pick.sec > 5.0) - { - found_part = true; - conveyor_parts_.erase(it); - break; - } - } - it = conveyor_parts_.erase(it); - } - else{ - ++it; - } - } - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 20000, "Waiting for %s %s to arrive on the conveyor", part_colors_[part_to_pick.color].c_str(), part_types_[part_to_pick.type].c_str()); - } // End lock_guard scope - } while (!found_part); - - // Correct robot position to account for part offset on conveyor - double part_rotation = GetYaw(part_pose); - geometry_msgs::msg::Pose robot_pose = floor_robot_.getCurrentPose().pose; - - std::vector waypoints; - waypoints.push_back(BuildPose(part_pose.position.x, robot_pose.position.y, - part_pose.position.z + 0.15, SetRobotOrientation(part_rotation))); - FloorRobotMoveCartesian(waypoints, 0.5, 0.5, true); - - auto elapsed_time_ = rclcpp::Time(now()) - detection_time; - auto current_part_position_ = part_pose.position.y - (elapsed_time.seconds() * conveyor_speed_); - // Check if part hasn't passed the pick location - if (current_part_position_ < 0) - { - RCLCPP_INFO(get_logger(), "Part has passed the pick location"); - found_part = false; - num_tries++; - continue; - } - - // Plan trajectory to pickup part - waypoints.clear(); - waypoints.push_back(BuildPose(part_pose.position.x, robot_pose.position.y, - part_pose.position.z + part_heights_[part_to_pick.type],SetRobotOrientation(part_rotation))); - - auto trajectory = FloorRobotPlanCartesian(waypoints, 0.5, 0.5, true); - if (!trajectory.first) - { - found_part = false; - num_tries++; - continue; - } - - // Wait for part to arrive at pick location - auto trajectory_time = trajectory.second.joint_trajectory.points.back().time_from_start; - while (rclcpp::Time(now()).nanoseconds() < (detection_time + elapsed_time + time_to_pick - trajectory_time).nanoseconds() - 2.75e8) - { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 20000, "Waiting for part to arrive at pick location"); - } - - // Execute trajectory to pickup part - FloorRobotSetGripperState(true); - floor_robot_.execute(trajectory.second); - - // Move up slightly - waypoints.clear(); - waypoints.push_back(BuildPose(part_pose.position.x, robot_pose.position.y, - part_pose.position.z + 0.1, SetRobotOrientation(0))); - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - if(floor_gripper_state_.attached) - { - // Add part to planning scene - std::string part_name = part_colors_[part_to_pick.color] + "_" + part_types_[part_to_pick.type]; - AddModelToPlanningScene(part_name, part_types_[part_to_pick.type] + ".stl", robot_pose); - RCLCPP_INFO_STREAM(get_logger(), "Attached " << part_name << " to robot"); - floor_robot_.attachObject(part_name); - order_planning_scene_objects_.push_back(part_name); - floor_robot_attached_part_ = part_to_pick; - part_picked = true; - } - - // Move up - waypoints.clear(); - waypoints.push_back(BuildPose(robot_pose.position.x, robot_pose.position.y, - robot_pose.position.z + 0.3, SetRobotOrientation(0))); - FloorRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - num_tries++; - } - - return true; -} - -bool TestCompetitor::FloorRobotPlacePartOnKitTray(int agv_num, int quadrant) -{ - if (!floor_gripper_state_.attached) - { - RCLCPP_ERROR(get_logger(), "No part attached"); - return false; - } - - RCLCPP_INFO_STREAM(get_logger(), "Placing the " << part_colors_[floor_robot_attached_part_.color] << " " << part_types_[floor_robot_attached_part_.type] << " on AGV " << agv_num << " in quadrant " << quadrant); - - // Move to agv - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_["agv" + std::to_string(agv_num)]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - // Determine target pose for part based on agv_tray pose - auto agv_tray_pose = FrameWorldPose("agv" + std::to_string(agv_num) + "_tray"); - - auto part_drop_offset = BuildPose(quad_offsets_[quadrant].first, quad_offsets_[quadrant].second, 0.0, - geometry_msgs::msg::Quaternion()); - - auto part_drop_pose = MultiplyPose(agv_tray_pose, part_drop_offset); - - std::vector waypoints; - - waypoints.push_back(BuildPose(part_drop_pose.position.x, part_drop_pose.position.y, - part_drop_pose.position.z + 0.3, SetRobotOrientation(0))); - - waypoints.push_back(BuildPose(part_drop_pose.position.x, part_drop_pose.position.y, - part_drop_pose.position.z + part_heights_[floor_robot_attached_part_.type] + kit_tray_thickness_ + drop_height_, - SetRobotOrientation(0))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.2, true); - - // Drop part in quadrant - FloorRobotSetGripperState(false); - - std::string part_name = part_colors_[floor_robot_attached_part_.color] + - "_" + part_types_[floor_robot_attached_part_.type]; - floor_robot_.detachObject(part_name); - - waypoints.clear(); - waypoints.push_back(BuildPose(part_drop_pose.position.x, part_drop_pose.position.y, - part_drop_pose.position.z + 0.2, - SetRobotOrientation(0))); - - FloorRobotMoveCartesian(waypoints, 0.2, 0.1, false); - - floor_robot_.setJointValueTarget("linear_actuator_joint", rail_positions_["agv" + std::to_string(agv_num)]); - floor_robot_.setJointValueTarget("floor_shoulder_pan_joint", 0); - FloorRobotMovetoTarget(); - - return true; -} - -void TestCompetitor::CeilingRobotSendHome() -{ - // Move ceiling robot to home joint state - RCLCPP_INFO_STREAM(get_logger(), "Moving ceiling robot to home position"); - ceiling_robot_.setNamedTarget("home"); - CeilingRobotMovetoTarget(); -} - -bool TestCompetitor::CeilingRobotSetGripperState(bool enable) -{ - if (ceiling_gripper_state_.enabled == enable) - { - if (ceiling_gripper_state_.enabled) - RCLCPP_INFO(get_logger(), "Already enabled"); - else - RCLCPP_INFO(get_logger(), "Already disabled"); - - return false; - } - - // Call enable service - auto request = std::make_shared(); - request->enable = enable; - - auto future = ceiling_robot_gripper_enable_->async_send_request(request); - future.wait(); - - if (!future.get()->success) - { - RCLCPP_ERROR(get_logger(), "Error calling gripper enable service"); - return false; - } - - return true; -} - -void TestCompetitor::CeilingRobotWaitForAttach(double timeout) -{ - // Wait for part to be attached - rclcpp::Time start = now(); - std::vector waypoints; - geometry_msgs::msg::Pose starting_pose = ceiling_robot_.getCurrentPose().pose; - - while (!ceiling_gripper_state_.attached) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for gripper attach"); - - waypoints.clear(); - starting_pose.position.z -= 0.001; - waypoints.push_back(starting_pose); - - CeilingRobotMoveCartesian(waypoints, 0.01, 0.01, false); - - usleep(500); - - if (now() - start > rclcpp::Duration::from_seconds(timeout)) - { - RCLCPP_ERROR(get_logger(), "Unable to pick up object"); - return; - } - } -} - -bool TestCompetitor::CeilingRobotWaitForAssemble(int station, ariac_msgs::msg::AssemblyPart part) -{ - // Wait for part to be attached - rclcpp::Time start = now(); - - auto rotation = GetYaw(FrameWorldPose("as"+std::to_string(station)+"_insert_frame")); - - bool assembled = false; - - double R[3][3] = {cos(rotation), -sin(rotation), 0, - sin(rotation), cos(rotation), 0, - 0, 0, 1}; - - double dx = R[0][0] * part.install_direction.x + R[0][1] * part.install_direction.y + R[0][2] * part.install_direction.z; - double dy = R[1][0] * part.install_direction.x + R[1][1] * part.install_direction.y + R[1][2] * part.install_direction.z; - double dz = R[2][0] * part.install_direction.x + R[2][1] * part.install_direction.y + R[2][2] * part.install_direction.z; - - geometry_msgs::msg::Pose pose = ceiling_robot_.getCurrentPose().pose; - - pose.position.x += dx * (assembly_offset_ + 0.005); - pose.position.y += dy * (assembly_offset_ + 0.005); - pose.position.z += dz * (assembly_offset_ + 0.005); - - std::vector waypoints; - waypoints.push_back(pose); - - auto ret = CeilingRobotPlanCartesian(waypoints, 0.01, 0.01, false); - - if (ret.first) { - ceiling_robot_.asyncExecute(ret.second); - } else { - return false; - } - - while (!assembled) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for part to be assembled"); - - // Check if part is assembled - switch (part.part.type) - { - case ariac_msgs::msg::Part::BATTERY: - assembled = assembly_station_states_[station].battery_attached; - break; - case ariac_msgs::msg::Part::PUMP: - assembled = assembly_station_states_[station].pump_attached; - break; - case ariac_msgs::msg::Part::SENSOR: - assembled = assembly_station_states_[station].sensor_attached; - break; - case ariac_msgs::msg::Part::REGULATOR: - assembled = assembly_station_states_[station].regulator_attached; - break; - default: - RCLCPP_WARN(get_logger(), "Not a valid part type"); - return false; - } - } - - ceiling_robot_.stop(); - - RCLCPP_INFO(get_logger(), "Part is assembled"); - - return true; -} - -bool TestCompetitor::CeilingRobotMovetoTarget() -{ - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = static_cast(ceiling_robot_.plan(plan)); - - if (success) - { - return static_cast(ceiling_robot_.execute(plan)); - } - else - { - RCLCPP_ERROR(get_logger(), "Unable to generate plan"); - return false; - } -} - -bool TestCompetitor::CeilingRobotMoveCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = ceiling_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return false; - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(ceiling_robot_.getCurrentState()->getRobotModel(), "ceiling_robot"); - rt.setRobotTrajectoryMsg(*ceiling_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return static_cast(ceiling_robot_.execute(trajectory)); -} - -std::pair TestCompetitor::CeilingRobotPlanCartesian( - std::vector waypoints, double vsf, double asf, bool avoid_collisions) -{ - moveit_msgs::msg::RobotTrajectory trajectory; - - double path_fraction = ceiling_robot_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory, avoid_collisions); - - if (path_fraction < 0.9) - { - RCLCPP_ERROR(get_logger(), "Unable to generate trajectory through waypoints"); - return std::make_pair(false, trajectory); - } - - // Retime trajectory - robot_trajectory::RobotTrajectory rt(ceiling_robot_.getCurrentState()->getRobotModel(), "ceiling_robot"); - rt.setRobotTrajectoryMsg(*ceiling_robot_.getCurrentState(), trajectory); - totg_.computeTimeStamps(rt, vsf, asf); - rt.getRobotTrajectoryMsg(trajectory); - - return std::make_pair(true, trajectory); -} - -bool TestCompetitor::CeilingRobotMoveToAssemblyStation(int station) -{ - RCLCPP_INFO_STREAM(get_logger(), "Moving ceiling robot to assembly station " << station); - switch (station) - { - case 1: - ceiling_robot_.setJointValueTarget(ceiling_as1_js_); - break; - case 2: - ceiling_robot_.setJointValueTarget(ceiling_as2_js_); - break; - case 3: - ceiling_robot_.setJointValueTarget(ceiling_as3_js_); - break; - case 4: - ceiling_robot_.setJointValueTarget(ceiling_as4_js_); - break; - default: - RCLCPP_WARN(get_logger(), "Not a valid assembly station"); - return false; - } - - return CeilingRobotMovetoTarget(); -} - -bool TestCompetitor::CeilingRobotPickAGVPart(ariac_msgs::msg::PartPose part) -{ - RCLCPP_INFO_STREAM(get_logger(), "Determining waypoints to pick " << part_types_[part.part.type]); - double part_rotation = GetYaw(part.pose); - std::vector waypoints; - - double dx = 0; - double dy = 0; - - if (part.part.type == ariac_msgs::msg::Part::BATTERY) - { - dx = battery_grip_offset_ * cos(part_rotation); - dy = battery_grip_offset_ * sin(part_rotation); - } - - waypoints.push_back(BuildPose(part.pose.position.x + dx, part.pose.position.y + dy, - part.pose.position.z + 0.4, SetRobotOrientation(part_rotation))); - - waypoints.push_back(BuildPose(part.pose.position.x + dx, part.pose.position.y + dy, - part.pose.position.z + part_heights_[part.part.type] + pick_offset_, SetRobotOrientation(part_rotation))); - - RCLCPP_INFO_STREAM(get_logger(), "Moving ceiling robot to pick " << part_types_[part.part.type]); - CeilingRobotMoveCartesian(waypoints, 0.7, 0.7, false); - - CeilingRobotSetGripperState(true); - - CeilingRobotWaitForAttach(5.0); - - // Add part to planning scene - std::string part_name = part_colors_[part.part.color] + "_" + part_types_[part.part.type]; - AddModelToPlanningScene(part_name, part_types_[part.part.type] + ".stl", part.pose); - ceiling_robot_.attachObject(part_name); - ceiling_robot_attached_part_ = part.part; - - // Move up slightly - waypoints.clear(); - waypoints.push_back(BuildPose(part.pose.position.x, part.pose.position.y, - part.pose.position.z + 0.3, - SetRobotOrientation(0))); - - CeilingRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - return true; -} - -bool TestCompetitor::CeilingRobotAssemblePart(int station, ariac_msgs::msg::AssemblyPart part) -{ - // Check that part is attached and matches part to assemble - if (!ceiling_gripper_state_.attached) - { - RCLCPP_WARN(get_logger(), "No part attached"); - return false; - } - - if (part.part != ceiling_robot_attached_part_) - { - RCLCPP_WARN(get_logger(), "Incorrect part attached for this assembly"); - return false; - } - - RCLCPP_INFO_STREAM(get_logger(),"Attempting to assemble a " << part_colors_[part.part.color] << " " << part_types_[part.part.type]); - // Calculate assembled pose in world frame - std::string insert_frame_name; - switch (station) - { - case 1: - insert_frame_name = "as1_insert_frame"; - break; - case 2: - insert_frame_name = "as2_insert_frame"; - break; - case 3: - insert_frame_name = "as3_insert_frame"; - break; - case 4: - insert_frame_name = "as4_insert_frame"; - break; - default: - RCLCPP_WARN(get_logger(), "Not a valid assembly station"); - return false; - } - - // Calculate robot positions at assembly and approach - KDL::Vector install(part.install_direction.x, part.install_direction.y, part.install_direction.z); - - KDL::Frame insert; - tf2::fromMsg(FrameWorldPose(insert_frame_name), insert); - - KDL::Frame part_assemble; - tf2::fromMsg(part.assembled_pose.pose, part_assemble); - - KDL::Frame part_to_gripper; - - // Build approach waypoints - std::vector waypoints; - if (part.part.type == ariac_msgs::msg::Part::BATTERY) - { - tf2::fromMsg(BuildPose(battery_grip_offset_, 0, part_heights_[part.part.type], QuaternionFromRPY(M_PI, 0, M_PI)), part_to_gripper); - KDL::Vector up(0, 0, 0.1); - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(up) * KDL::Frame(install * -0.06) * part_assemble * part_to_gripper)); - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(install * -0.06) * part_assemble * part_to_gripper)); - } - else - { - tf2::fromMsg(BuildPose(0, 0, part_heights_[part.part.type], QuaternionFromRPY(M_PI, 0, M_PI)), part_to_gripper); - - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(install * -0.1) * part_assemble * part_to_gripper)); - } - - // Move to approach position - CeilingRobotMoveCartesian(waypoints, 0.3, 0.3, true); - - // Move to just before assembly position - waypoints.clear(); - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(install * -assembly_offset_) * part_assemble * part_to_gripper)); - CeilingRobotMoveCartesian(waypoints, 0.1, 0.1, true); - - CeilingRobotWaitForAssemble(station, part); - - CeilingRobotSetGripperState(false); - - std::string part_name = part_colors_[ceiling_robot_attached_part_.color] + - "_" + part_types_[ceiling_robot_attached_part_.type]; - ceiling_robot_.detachObject(part_name); - - waypoints.clear(); - KDL::Vector away; - if (part.part.type == ariac_msgs::msg::Part::REGULATOR) { - away = KDL::Vector(-0.1, 0, 0); - } else { - away = KDL::Vector(0, 0, 0.1); - } - - waypoints.push_back(tf2::toMsg(insert * KDL::Frame(away) * part_assemble * part_to_gripper)); - - CeilingRobotMoveCartesian(waypoints, 0.1, 0.1, false); - - return true; -} - -bool TestCompetitor::CompleteOrders() -{ - // Wait for first order to be published - while (orders_.size() == 0) - { - } - - bool success; - while (true) - { - if (competition_state_ == ariac_msgs::msg::CompetitionState::ENDED) - { - success = false; - break; - } - - if (orders_.size() == 0) - { - if (competition_state_ != ariac_msgs::msg::CompetitionState::ORDER_ANNOUNCEMENTS_DONE) - { - // wait for more orders - RCLCPP_INFO(get_logger(), "Waiting for orders..."); - while (orders_.size() == 0) - { - } - } - else - { - RCLCPP_INFO(get_logger(), "Completed all orders"); - success = true; - break; - } - } - - current_order_ = orders_.front(); - orders_.erase(orders_.begin()); - // int kitting_agv_num = -1; - - if (current_order_.type == ariac_msgs::msg::Order::KITTING) - { - RCLCPP_INFO_STREAM(get_logger(), "Current order is: " << current_order_.id << " of type KITTING"); - TestCompetitor::CompleteKittingTask(current_order_.kitting_task); - } - else if (current_order_.type == ariac_msgs::msg::Order::ASSEMBLY) - { - RCLCPP_INFO_STREAM(get_logger(), "Current order is: " << current_order_.id << " of type ASSEMBLY"); - TestCompetitor::CompleteAssemblyTask(current_order_.assembly_task); - } - else if (current_order_.type == ariac_msgs::msg::Order::COMBINED) - { - RCLCPP_INFO_STREAM(get_logger(), "Current order is: " << current_order_.id << " of type COMBINED"); - TestCompetitor::CompleteCombinedTask(current_order_.combined_task); - } - - - TestCompetitor::SubmitOrder(current_order_.id); - } - return success; -} - - -bool TestCompetitor::CompleteKittingTask(ariac_msgs::msg::KittingTask task) -{ - FloorRobotSendHome(); - - if(agv_locations_[task.agv_number] != ariac_msgs::msg::AGVStatus::KITTING) - { - MoveAGV(task.agv_number,ariac_msgs::srv::MoveAGV::Request::KITTING); - } - - FloorRobotPickandPlaceTray(task.tray_id, task.agv_number); - - bool found; - for (auto kit_part : task.parts) - { - found = FloorRobotPickBinPart(kit_part.part); - if (!found) - { - FloorRobotPickConveyorPart(kit_part.part); - } - FloorRobotPlacePartOnKitTray(task.agv_number, kit_part.quadrant); - } - - // Check quality - auto request = std::make_shared(); - request->order_id = current_order_.id; - auto future = quality_checker_->async_send_request(request); - future.wait(); - - if (!future.get()->all_passed) - { - RCLCPP_ERROR(get_logger(), "Issue with shipment"); - } - - //Remove objects from planning scene - planning_scene_.removeCollisionObjects(order_planning_scene_objects_); - - order_planning_scene_objects_.clear(); - - MoveAGV(task.agv_number, task.destination); - - return true; -} - -bool TestCompetitor::CompleteAssemblyTask(ariac_msgs::msg::AssemblyTask task) -{ - // Send AGVs to assembly station - for (auto const &agv : task.agv_numbers) - { - int destination; - if (task.station == ariac_msgs::msg::AssemblyTask::AS1 || task.station == ariac_msgs::msg::AssemblyTask::AS3) - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_FRONT; - } - else if (task.station == ariac_msgs::msg::AssemblyTask::AS2 || task.station == ariac_msgs::msg::AssemblyTask::AS4) - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_BACK; - } - - LockAGVTray(agv); - MoveAGV(agv, destination); - } - - CeilingRobotMoveToAssemblyStation(task.station); - - // Get Assembly Poses - RCLCPP_INFO_STREAM(get_logger(), "Getting pre assembly poses for order " << current_order_.id); - auto request = std::make_shared(); - request->order_id = current_order_.id; - auto future = pre_assembly_poses_getter_->async_send_request(request); - - RCLCPP_INFO(get_logger(), "Waiting for pre assembly poses"); - - future.wait(); - - RCLCPP_INFO(get_logger(), "Recieved pre assembly poses"); - - std::vector agv_part_poses; - - auto result = future.get(); - - if (result->valid_id) - { - RCLCPP_INFO(get_logger(), "Valid id"); - - try { - int len = result->parts.size(); - RCLCPP_INFO_STREAM(get_logger(), "There are " << std::to_string(len) << " parts"); - } - catch (...) { - RCLCPP_INFO(get_logger(), "Unable to access future result"); - } - - agv_part_poses = result->parts; - - if (agv_part_poses.size() == 0) - { - RCLCPP_WARN(get_logger(), "No part poses recieved"); - return false; - } - } - else - { - RCLCPP_WARN(get_logger(), "Not a valid order ID"); - return false; - } - - for (auto const &part_to_assemble : task.parts) - { - // Check if matching part exists in agv_parts - bool part_exists = false; - ariac_msgs::msg::PartPose part_to_pick; - part_to_pick.part = part_to_assemble.part; - for (auto const &agv_part : agv_part_poses) - { - if (agv_part.part.type == part_to_assemble.part.type && agv_part.part.color == part_to_assemble.part.color) - { - part_exists = true; - part_to_pick.pose = agv_part.pose; - break; - } - } - - if (!part_exists) - { - RCLCPP_WARN_STREAM(get_logger(), "Part with type: " << part_to_assemble.part.type << " and color: " << part_to_assemble.part.color << " not found on tray"); - continue; - } - - // Pick up part - CeilingRobotPickAGVPart(part_to_pick); - - CeilingRobotMoveToAssemblyStation(task.station); - - // Assemble Part to insert - CeilingRobotAssemblePart(task.station, part_to_assemble); - - CeilingRobotMoveToAssemblyStation(task.station); - } - - return true; -} - -bool TestCompetitor::CompleteCombinedTask(ariac_msgs::msg::CombinedTask task) -{ - RCLCPP_INFO_STREAM(get_logger(),"Starting Combined Task"); - // Decide on a tray to use - int id; - if (kts1_trays_.size() != 0) - { - id = kts1_trays_[0].id; - RCLCPP_INFO_STREAM(get_logger(), "Using tray " << id << " from kts1"); - } - else if (kts2_trays_.size() != 0) - { - id = kts2_trays_[0].id; - RCLCPP_INFO_STREAM(get_logger(), "Using tray " << id << " from kts2"); - } - else - { - RCLCPP_ERROR(get_logger(), "No trays available."); - return false; - } - - // Decide which AGV to use - int agv_number; - if (task.station == ariac_msgs::msg::CombinedTask::AS1 or task.station == ariac_msgs::msg::CombinedTask::AS2) - { - agv_number = 1; - RCLCPP_INFO_STREAM(get_logger(),"Using AGV " << agv_number << " for assembly since station is AS1 or AS2"); - } - else - { - agv_number = 4; - RCLCPP_INFO_STREAM(get_logger(),"Using AGV " << agv_number << " for assembly since station is AS3 or AS4"); - } - - MoveAGV(agv_number, ariac_msgs::srv::MoveAGV::Request::KITTING); - - FloorRobotPickandPlaceTray(id, agv_number); - - int count = 1; - bool found; - for (auto assembly_part : task.parts) - { - found = FloorRobotPickBinPart(assembly_part.part); - if (!found) - { - FloorRobotPickConveyorPart(assembly_part.part); - } - FloorRobotPlacePartOnKitTray(agv_number, count); - count++; - } - - int destination; - if (task.station == ariac_msgs::msg::CombinedTask::AS1 or task.station == ariac_msgs::msg::CombinedTask::AS3) - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_FRONT; - } - else - { - destination = ariac_msgs::srv::MoveAGV::Request::ASSEMBLY_BACK; - } - - //Remove objects from planning scene - planning_scene_.removeCollisionObjects(order_planning_scene_objects_); - - order_planning_scene_objects_.clear(); - - MoveAGV(agv_number, destination); - - CeilingRobotMoveToAssemblyStation(task.station); - - // Get Assembly Poses - RCLCPP_INFO_STREAM(get_logger(), "Getting pre assembly poses for order " << current_order_.id); - auto request = std::make_shared(); - request->order_id = current_order_.id; - auto future = pre_assembly_poses_getter_->async_send_request(request); - - future.wait(); - - auto result = future.get(); - - std::vector agv_part_poses; - if (result->valid_id) - { - agv_part_poses = result->parts; - - if (agv_part_poses.size() == 0) - { - RCLCPP_WARN(get_logger(), "No part poses recieved"); - return false; - } - } - else - { - RCLCPP_WARN(get_logger(), "Not a valid order ID"); - return false; - } - - for (auto const &part_to_assemble : task.parts) - { - // Check if matching part exists in agv_parts - bool part_exists = false; - ariac_msgs::msg::PartPose part_to_pick; - part_to_pick.part = part_to_assemble.part; - for (auto const &agv_part : agv_part_poses) - { - if (agv_part.part.type == part_to_assemble.part.type && agv_part.part.color == part_to_assemble.part.color) - { - part_exists = true; - part_to_pick.pose = agv_part.pose; - break; - } - } - - if (!part_exists) - { - RCLCPP_WARN_STREAM(get_logger(), "Part with type: " << part_types_[part_to_assemble.part.type] << " and color: " << part_colors_[part_to_assemble.part.color] << " not found on tray"); - continue; - } - - // Pick up part - CeilingRobotPickAGVPart(part_to_pick); - - CeilingRobotMoveToAssemblyStation(task.station); - - // Assemble Part to insert - CeilingRobotAssemblePart(task.station, part_to_assemble); - - CeilingRobotMoveToAssemblyStation(task.station); - } - - return true; -} - -bool TestCompetitor::StartCompetition() -{ - // Wait for competition state to be ready - while (competition_state_ != ariac_msgs::msg::CompetitionState::READY) - { - } - - RCLCPP_INFO_STREAM(get_logger(), "Starting competition"); - - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/start_competition"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::EndCompetition() -{ - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/end_competition"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - RCLCPP_INFO(get_logger(), "Ending competition."); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::SubmitOrder(std::string order_id) -{ - RCLCPP_INFO_STREAM(get_logger(), "Submitting order " << order_id); - rclcpp::Client::SharedPtr client; - std::string srv_name = "/ariac/submit_order"; - client = this->create_client(srv_name); - auto request = std::make_shared(); - request->order_id = order_id; - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::LockAGVTray(int agv_num) -{ - RCLCPP_INFO_STREAM(get_logger(), "Locking Tray to AGV" << agv_num); - - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/agv" + std::to_string(agv_num) + "_lock_tray"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::UnlockAGVTray(int agv_num) -{ - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/agv" + std::to_string(agv_num) + "_unlock_tray"; - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - - auto future = client->async_send_request(request); - future.wait(); - - return future.get()->success; -} - -bool TestCompetitor::MoveAGV(int agv_num, int destination) -{ - RCLCPP_INFO_STREAM(get_logger(), "Moving AGV" << agv_num << " to " << agv_destination_[destination]); - rclcpp::Client::SharedPtr client; - - std::string srv_name = "/ariac/move_agv" + std::to_string(agv_num); - - client = this->create_client(srv_name); - - auto request = std::make_shared(); - request->location = destination; - - auto future = client->async_send_request(request); - - rclcpp::Duration timeout = rclcpp::Duration::from_seconds(20); - - rclcpp::Time start = get_clock()->now(); - - while (get_clock()->now() - start < timeout){ - if (agv_locations_[agv_num] == destination) { - return true; - } - } - - RCLCPP_INFO_STREAM(get_logger(), "Unable to move AGV" << agv_num << " to " << agv_destination_[destination]); - return false; -} \ No newline at end of file