diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index eb0cecc..8350ef5 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -21,7 +21,7 @@ jobs: matrix: distro: [humble] container: - image: ghcr.io/ros-industrial-consortium/scan_n_plan_workshop:${{ matrix.distro }}-4.6 + image: ghcr.io/ros-industrial-consortium/scan_n_plan_workshop:${{ matrix.distro }}-5.0 env: CCACHE_DIR: ${{ github.workspace }}/${{ matrix.distro }}/.ccache DEBIAN_FRONTEND: noninteractive diff --git a/snp_automate_2023/CMakeLists.txt b/CMakeLists.txt similarity index 100% rename from snp_automate_2023/CMakeLists.txt rename to CMakeLists.txt diff --git a/snp_automate_2023/calibration/cal_data_collection.launch.xml b/calibration/cal_data_collection.launch.xml similarity index 100% rename from snp_automate_2023/calibration/cal_data_collection.launch.xml rename to calibration/cal_data_collection.launch.xml diff --git a/snp_automate_2023/calibration/calibrate.launch.xml b/calibration/calibrate.launch.xml similarity index 100% rename from snp_automate_2023/calibration/calibrate.launch.xml rename to calibration/calibrate.launch.xml diff --git a/snp_automate_2023/calibration/charuco_target_7x5.yaml b/calibration/charuco_target_7x5.yaml similarity index 100% rename from snp_automate_2023/calibration/charuco_target_7x5.yaml rename to calibration/charuco_target_7x5.yaml diff --git a/snp_automate_2023/calibration/intrinsics.yaml b/calibration/intrinsics.yaml similarity index 100% rename from snp_automate_2023/calibration/intrinsics.yaml rename to calibration/intrinsics.yaml diff --git a/snp_automate_2023/calibration/modified_circle_grid_target.yaml b/calibration/modified_circle_grid_target.yaml similarity index 100% rename from snp_automate_2023/calibration/modified_circle_grid_target.yaml rename to calibration/modified_circle_grid_target.yaml diff --git a/snp_automate_2023/config/app.rviz b/config/app.rviz similarity index 99% rename from snp_automate_2023/config/app.rviz rename to config/app.rviz index e5eb499..2ceb718 100644 --- a/snp_automate_2023/config/app.rviz +++ b/config/app.rviz @@ -22,8 +22,8 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: snp_automate_application/AutomateApplication - Name: AutomateApplication + - Class: snp/SNPApplication + Name: SNPApplication - Class: rviz_common/Displays Help Height: 70 Name: Displays @@ -565,7 +565,7 @@ Visualization Manager: Yaw: 1.3503996133804321 Saved: ~ Window Geometry: - AutomateApplication: + SNPApplication: collapsed: false Displays: collapsed: false diff --git a/snp_automate_2023/config/calibration.yaml b/config/calibration.yaml similarity index 100% rename from snp_automate_2023/config/calibration.yaml rename to config/calibration.yaml diff --git a/snp_automate_2023/config/controllers.yaml b/config/controllers.yaml similarity index 100% rename from snp_automate_2023/config/controllers.yaml rename to config/controllers.yaml diff --git a/snp_automate_2023/config/rviz_base_config.rviz b/config/rviz_base_config.rviz similarity index 100% rename from snp_automate_2023/config/rviz_base_config.rviz rename to config/rviz_base_config.rviz diff --git a/snp_automate_2023/config/scan_traj.yaml b/config/scan_traj.yaml similarity index 100% rename from snp_automate_2023/config/scan_traj.yaml rename to config/scan_traj.yaml diff --git a/snp_automate_application/config/snp_automate.btproj b/config/snp_automate.btproj similarity index 100% rename from snp_automate_application/config/snp_automate.btproj rename to config/snp_automate.btproj diff --git a/snp_automate_application/config/snp_automate.xml b/config/snp_automate.xml similarity index 100% rename from snp_automate_application/config/snp_automate.xml rename to config/snp_automate.xml diff --git a/snp_automate_2023/config/tpp.yaml b/config/tpp.yaml similarity index 100% rename from snp_automate_2023/config/tpp.yaml rename to config/tpp.yaml diff --git a/snp_automate_2023/config/workcell.srdf b/config/workcell.srdf similarity index 100% rename from snp_automate_2023/config/workcell.srdf rename to config/workcell.srdf diff --git a/snp_automate_2023/config/workcell_plugins.yaml b/config/workcell_plugins.yaml similarity index 100% rename from snp_automate_2023/config/workcell_plugins.yaml rename to config/workcell_plugins.yaml diff --git a/dependencies.repos b/dependencies.repos index bb73ec1..fd3c280 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -14,3 +14,7 @@ local-name: motoros2_fjt_pt_queue_proxy uri: https://github.com/gavanderhoorn/motoros2_fjt_pt_queue_proxy.git version: 9279969b6eec46b82fd4867ebeabfe534d653b6f +- git: + local-name: motoros2_behavior_tree + uri: https://github.com/marip8/motoros2_behavior_tree.git + version: 0.1.0 diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 42fecfb..5a5bb01 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -4,7 +4,7 @@ services: context: .. dockerfile: docker/Dockerfile args: - - TAG=humble-4.6 + - TAG=humble-5.0 environment: DISPLAY: $DISPLAY XAUTHORITY: $XAUTHORITY diff --git a/snp_automate_2023/launch/start.launch.xml b/launch/start.launch.xml similarity index 93% rename from snp_automate_2023/launch/start.launch.xml rename to launch/start.launch.xml index 4c4acc1..3e716c9 100644 --- a/snp_automate_2023/launch/start.launch.xml +++ b/launch/start.launch.xml @@ -30,13 +30,16 @@ - + + + + - + @@ -114,7 +117,7 @@ - + diff --git a/snp_automate_2023/launch/test.launch.xml b/launch/test.launch.xml similarity index 100% rename from snp_automate_2023/launch/test.launch.xml rename to launch/test.launch.xml diff --git a/snp_automate_2023/meshes/collision/ati_bracket.ply b/meshes/collision/ati_bracket.ply similarity index 100% rename from snp_automate_2023/meshes/collision/ati_bracket.ply rename to meshes/collision/ati_bracket.ply diff --git a/snp_automate_2023/meshes/collision/ati_hose.ply b/meshes/collision/ati_hose.ply similarity index 100% rename from snp_automate_2023/meshes/collision/ati_hose.ply rename to meshes/collision/ati_hose.ply diff --git a/snp_automate_2023/meshes/collision/ati_spindle.ply b/meshes/collision/ati_spindle.ply similarity index 100% rename from snp_automate_2023/meshes/collision/ati_spindle.ply rename to meshes/collision/ati_spindle.ply diff --git a/snp_automate_2023/meshes/collision/ati_standoff.ply b/meshes/collision/ati_standoff.ply similarity index 100% rename from snp_automate_2023/meshes/collision/ati_standoff.ply rename to meshes/collision/ati_standoff.ply diff --git a/snp_automate_2023/meshes/collision/ati_tool.ply b/meshes/collision/ati_tool.ply similarity index 100% rename from snp_automate_2023/meshes/collision/ati_tool.ply rename to meshes/collision/ati_tool.ply diff --git a/snp_automate_2023/meshes/collision/hc10_standoff.ply b/meshes/collision/hc10_standoff.ply similarity index 100% rename from snp_automate_2023/meshes/collision/hc10_standoff.ply rename to meshes/collision/hc10_standoff.ply diff --git a/snp_automate_2023/meshes/collision/sensor_bracket.ply b/meshes/collision/sensor_bracket.ply similarity index 100% rename from snp_automate_2023/meshes/collision/sensor_bracket.ply rename to meshes/collision/sensor_bracket.ply diff --git a/snp_automate_2023/meshes/collision/sponge.ply b/meshes/collision/sponge.ply similarity index 100% rename from snp_automate_2023/meshes/collision/sponge.ply rename to meshes/collision/sponge.ply diff --git a/snp_automate_2023/meshes/collision/steel_table.ply b/meshes/collision/steel_table.ply similarity index 100% rename from snp_automate_2023/meshes/collision/steel_table.ply rename to meshes/collision/steel_table.ply diff --git a/snp_automate_2023/meshes/part_scan.ply b/meshes/part_scan.ply similarity index 100% rename from snp_automate_2023/meshes/part_scan.ply rename to meshes/part_scan.ply diff --git a/snp_automate_2023/meshes/visual/steel_table.ply b/meshes/visual/steel_table.ply similarity index 100% rename from snp_automate_2023/meshes/visual/steel_table.ply rename to meshes/visual/steel_table.ply diff --git a/snp_automate_2023/motoros2/motoros2_config.yaml b/motoros2/motoros2_config.yaml similarity index 100% rename from snp_automate_2023/motoros2/motoros2_config.yaml rename to motoros2/motoros2_config.yaml diff --git a/snp_automate_2023/package.xml b/package.xml similarity index 95% rename from snp_automate_2023/package.xml rename to package.xml index bcd6192..594c304 100644 --- a/snp_automate_2023/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ controller_manager joint_trajectory_controller joint_state_broadcaster + motoros2_behavior_tree ament_cmake diff --git a/snp_automate_application/CMakeLists.txt b/snp_automate_application/CMakeLists.txt deleted file mode 100644 index 171ddfc..0000000 --- a/snp_automate_application/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(snp_automate_application) - -# Default to C++17 -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(CMAKE_AUTOMOC ON) -set(CMAKE_AUTORCC ON) -set(CMAKE_AUTOUIC ON) - -find_package(ament_cmake REQUIRED) -find_package(snp_application REQUIRED) -find_package(motoros2_interfaces REQUIRED) -find_package(rviz_common REQUIRED) -find_package(pluginlib REQUIRED) - -add_library(${PROJECT_NAME}_widget SHARED src/snp_automate_widget.cpp src/bt/snp_automate_bt_ros_nodes.cpp) -target_include_directories(${PROJECT_NAME}_widget PUBLIC "$" - "$") -target_link_libraries(${PROJECT_NAME}_widget Qt5::Widgets) -ament_target_dependencies(${PROJECT_NAME}_widget motoros2_interfaces snp_application) - -add_library(${PROJECT_NAME}_panel SHARED src/snp_automate_panel.cpp) -target_link_libraries(${PROJECT_NAME}_panel ${PROJECT_NAME}_widget) -ament_target_dependencies(${PROJECT_NAME}_panel rviz_common pluginlib) - -# Install the headers -install(DIRECTORY include/${PROJECT_NAME} DESTINATION include/) - -# Install the library(ies) -install(TARGETS ${PROJECT_NAME}_widget ${PROJECT_NAME}_panel EXPORT ${PROJECT_NAME}-targets DESTINATION lib) -ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET) - -# Install MotoROS2 simulator -install(PROGRAMS scripts/motoros2_simulator.py DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/) - -pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) -ament_export_dependencies( - snp_application - motoros2_interfaces - rviz_common - pluginlib) -ament_package() diff --git a/snp_automate_application/include/snp_automate_application/bt/snp_automate_bt_ros_nodes.h b/snp_automate_application/include/snp_automate_application/bt/snp_automate_bt_ros_nodes.h deleted file mode 100644 index 40940cf..0000000 --- a/snp_automate_application/include/snp_automate_application/bt/snp_automate_bt_ros_nodes.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace snp_automate_application -{ -class StartTrajModeNode : public snp_application::SnpRosServiceNode -{ -public: - using snp_application::SnpRosServiceNode::providedPorts; - using snp_application::SnpRosServiceNode::SnpRosServiceNode; - - bool setRequest(typename Request::SharedPtr& request) override; - BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; -}; - -class StartPointQueueModeNode : public snp_application::SnpRosServiceNode -{ -public: - using snp_application::SnpRosServiceNode::providedPorts; - using snp_application::SnpRosServiceNode::SnpRosServiceNode; - - bool setRequest(typename Request::SharedPtr& request) override; - BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) override; -}; - -} // namespace snp_automate_application diff --git a/snp_automate_application/include/snp_automate_application/snp_automate_widget.h b/snp_automate_application/include/snp_automate_application/snp_automate_widget.h deleted file mode 100644 index 143620f..0000000 --- a/snp_automate_application/include/snp_automate_application/snp_automate_widget.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#include - -namespace snp_automate_application -{ -class AutomateWidget : public snp_application::SNPWidget -{ -public: - explicit AutomateWidget(rclcpp::Node::SharedPtr rviz_node, QWidget* parent = nullptr); - -protected: - BT::BehaviorTreeFactory createBTFactory(int ros_short_timeout, int ros_long_timeout) override; -}; - -} // namespace snp_automate_application diff --git a/snp_automate_application/package.xml b/snp_automate_application/package.xml deleted file mode 100644 index fe31c0c..0000000 --- a/snp_automate_application/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - snp_automate_application - 0.0.0 - - SNP Automate 2023 Application package - - Michael Ripperger - Apache 2.0 - - ament_cmake - - snp_application - rviz_common - pluginlib - motoros2_interfaces - - ament_lint_auto - - - - ament_cmake - - diff --git a/snp_automate_application/plugin_description.xml b/snp_automate_application/plugin_description.xml deleted file mode 100644 index faddd7e..0000000 --- a/snp_automate_application/plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Scan N Plan application panel for Automate - - - diff --git a/snp_automate_application/scripts/motoros2_simulator.py b/snp_automate_application/scripts/motoros2_simulator.py deleted file mode 100755 index 20f14b7..0000000 --- a/snp_automate_application/scripts/motoros2_simulator.py +++ /dev/null @@ -1,38 +0,0 @@ -#! /usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from motoros2_interfaces.srv import StartTrajMode, ReadSingleIO, WriteSingleIO, StartPointQueueMode -from motoros2_interfaces.msg import MotionReadyEnum -from std_srvs.srv import Trigger - -class RobotServiceNode(Node): - def __init__(self): - super().__init__('motoros2_simulator') - - self.create_service(StartTrajMode, 'start_traj_mode', self.enable_robot_callback) - self.create_service(Trigger, 'stop_traj_mode', self.disable_robot_callback) - self.create_service(StartPointQueueMode, 'start_point_queue_mode', self.enable_robot_callback) - self.io = {} - - async def enable_robot_callback(self, request, response): - response.message = 'Robot enabled' - response.result_code = MotionReadyEnum() - response.result_code.value = MotionReadyEnum.READY - return response - - - async def disable_robot_callback(self, request, response): - response.message = 'Robot disabled' - response.success = True - return response - -def main(args=None): - rclpy.init(args=args) - node = RobotServiceNode() - node.get_logger().info('Started MotoROS2 simulator') - rclpy.spin(node) - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/snp_automate_application/src/bt/snp_automate_bt_ros_nodes.cpp b/snp_automate_application/src/bt/snp_automate_bt_ros_nodes.cpp deleted file mode 100644 index f65e8d6..0000000 --- a/snp_automate_application/src/bt/snp_automate_bt_ros_nodes.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include - -namespace snp_automate_application -{ -bool StartTrajModeNode::setRequest(typename Request::SharedPtr& /*request*/) -{ - return true; -} - -BT::NodeStatus StartTrajModeNode::onResponseReceived(const typename Response::SharedPtr& response) -{ - switch(response->result_code.value) - { - case motoros2_interfaces::msg::MotionReadyEnum::READY: - return BT::NodeStatus::SUCCESS; - default: - config().blackboard->set(snp_application::ERROR_MESSAGE_KEY, response->message); - return BT::NodeStatus::FAILURE; - } -} - -bool StartPointQueueModeNode::setRequest(typename Request::SharedPtr& /*request*/) -{ - return true; -} - -BT::NodeStatus StartPointQueueModeNode::onResponseReceived(const typename Response::SharedPtr& response) -{ - switch(response->result_code.value) - { - case motoros2_interfaces::msg::MotionReadyEnum::READY: - return BT::NodeStatus::SUCCESS; - default: - config().blackboard->set(snp_application::ERROR_MESSAGE_KEY, response->message); - return BT::NodeStatus::FAILURE; - } -} - -} // namespace snp_application diff --git a/snp_automate_application/src/snp_automate_panel.cpp b/snp_automate_application/src/snp_automate_panel.cpp deleted file mode 100644 index 03fbeb3..0000000 --- a/snp_automate_application/src/snp_automate_panel.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include - -#include -#include -#include -#include - -namespace snp_automate_application -{ -class AutomatePanel : public rviz_common::Panel -{ -public: - void onInitialize() override - { - try - { - auto rviz_node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - auto layout = new QVBoxLayout(); - layout->addWidget(new AutomateWidget(rviz_node, this)); - setLayout(layout); - } - catch (const std::exception& ex) - { - QMessageBox::warning(this, "Error", ex.what()); - } - } -}; - -} // namespace snp_automate_application - -#include -PLUGINLIB_EXPORT_CLASS(snp_automate_application::AutomatePanel, rviz_common::Panel) diff --git a/snp_automate_application/src/snp_automate_widget.cpp b/snp_automate_application/src/snp_automate_widget.cpp deleted file mode 100644 index 3846bd6..0000000 --- a/snp_automate_application/src/snp_automate_widget.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include - -namespace snp_automate_application -{ -AutomateWidget::AutomateWidget(rclcpp::Node::SharedPtr rviz_node, QWidget* parent) - : snp_application::SNPWidget(rviz_node, parent) -{ - -} - -BT::BehaviorTreeFactory AutomateWidget::createBTFactory(int ros_short_timeout, int ros_long_timeout) -{ - BT::BehaviorTreeFactory factory = SNPWidget::createBTFactory(ros_short_timeout, ros_long_timeout); - - BT::RosNodeParams ros_params; - ros_params.nh = bt_node_; - ros_params.wait_for_server_timeout = std::chrono::seconds(0); - ros_params.server_timeout = std::chrono::seconds(ros_short_timeout); - - factory.registerNodeType("StartTrajMode", ros_params); - factory.registerNodeType("StartPointQueueMode", ros_params); - - return factory; -} - -} // namespace snp_automate_application diff --git a/snp_automate_2023/urdf/ros2_control.xacro b/urdf/ros2_control.xacro similarity index 100% rename from snp_automate_2023/urdf/ros2_control.xacro rename to urdf/ros2_control.xacro diff --git a/snp_automate_2023/urdf/workcell.xacro b/urdf/workcell.xacro similarity index 100% rename from snp_automate_2023/urdf/workcell.xacro rename to urdf/workcell.xacro