├── .gitignore ├── .travis.yml ├── README.md ├── godel.rosinstall ├── godel ├── CMakeLists.txt └── package.xml ├── godel_msgs ├── CMakeLists.txt ├── action │ ├── BlendProcessExecution.action │ ├── PathPlanning.action │ ├── ProcessExecution.action │ ├── ProcessPlanning.action │ └── SelectMotionPlan.action ├── msg │ ├── BlendingPlanParameters.msg │ ├── PathPlanningParameters.msg │ ├── ProcessPath.msg │ ├── ProcessPlan.msg │ ├── RobotScanParameters.msg │ ├── ScanPlanParameters.msg │ ├── SelectedSurfacesChanged.msg │ ├── SurfaceBoundaries.msg │ └── SurfaceDetectionParameters.msg ├── package.xml └── srv │ ├── BlendProcessPlanning.srv │ ├── BlendingPlan.srv │ ├── EnsensoCommand.srv │ ├── GetAvailableMotionPlans.srv │ ├── KeyenceProcessPlanning.srv │ ├── LoadSaveMotionPlan.srv │ ├── OffsetBoundary.srv │ ├── PathPlanning.srv │ ├── ProcessPlanning.srv │ ├── RenameSurface.srv │ ├── SelectMotionPlan.srv │ ├── SelectSurface.srv │ ├── SurfaceBlendingParameters.srv │ ├── SurfaceDetection.srv │ ├── TrajectoryExecution.srv │ └── TrajectoryPlanning.srv ├── godel_param_helpers ├── CMakeLists.txt ├── include │ └── godel_param_helpers │ │ └── godel_param_helpers.h └── package.xml ├── godel_path_execution ├── CMakeLists.txt ├── include │ └── godel_path_execution │ │ └── path_execution_service.h ├── launch │ └── path_execution_service.launch ├── package.xml └── src │ ├── path_execution_service.cpp │ └── path_execution_service_node.cpp ├── godel_plugins ├── CMakeLists.txt ├── include │ └── godel_plugins │ │ ├── rqt_plugins │ │ └── robot_blending_plugin.h │ │ ├── rviz_panels │ │ └── robot_blending_panel.h │ │ └── widgets │ │ ├── parameter_window_base.h │ │ ├── path_planning_param_window.h │ │ ├── robot_blending_widget.h │ │ ├── robot_scan_configuration.h │ │ ├── scan_tool_configuration_window.h │ │ └── surface_detection_configuration.h ├── package.xml ├── plugin.xml ├── rqt │ └── robot_blending.perspective └── src │ ├── rqt_plugins │ └── robot_blending_plugin.cpp │ ├── rviz_panels │ └── robot_blending_panel.cpp │ └── widgets │ ├── parameter_window_base.cpp │ ├── path_planning_param_window.cpp │ ├── path_planning_param_window.ui │ ├── pose_widget.ui │ ├── robot_blending_plugin.ui │ ├── robot_blending_widget.cpp │ ├── robot_scan_configuration.cpp │ ├── robot_scan_configuration.ui │ ├── scan_tool_configuration_window.cpp │ ├── scan_tool_configuration_window.ui │ ├── surface_detection_configuration.cpp │ └── surface_detection_configuration.ui ├── godel_polygon_offset ├── CMakeLists.txt ├── README.md ├── cmake │ └── FindOpenVoronoi.cmake ├── include │ └── godel_polygon_offset │ │ └── polygon_offset.h ├── package.xml ├── src │ ├── offset_sorter.patch │ ├── polygon_offset.cpp │ └── polygon_offset_node.cpp └── srv │ └── OffsetPolygon.srv ├── godel_process_execution ├── CMakeLists.txt ├── include │ └── godel_process_execution │ │ ├── abb_blend_process_service.h │ │ ├── blend_process_service.h │ │ └── keyence_process_service.h ├── launch │ ├── abb_blend_process_execution.launch │ ├── blend_process_execution.launch │ └── keyence_process_execution.launch ├── package.xml └── src │ ├── abb_blend_process_service.cpp │ ├── abb_blend_process_service_node.cpp │ ├── blend_process_service.cpp │ ├── blend_process_service_node.cpp │ ├── keyence_process_service.cpp │ ├── keyence_process_service_node.cpp │ ├── process_utils.cpp │ └── process_utils.h ├── godel_process_path_generation ├── CMakeLists.txt ├── README.md ├── include │ └── godel_process_path_generation │ │ ├── get_boundary.h │ │ ├── polygon_pts.hpp │ │ ├── polygon_utils.h │ │ ├── process_path.h │ │ ├── process_path_generator.h │ │ ├── process_pt.h │ │ ├── process_transition.h │ │ └── utils.h ├── package.xml ├── src │ ├── polygon_utils.cpp │ ├── process_path.cpp │ ├── process_path_generator.cpp │ ├── process_path_generator_node.cpp │ ├── process_path_visualization_node.cpp │ ├── process_pt.cpp │ └── process_transition.cpp ├── srv │ └── VisualizeBlendingPlan.srv └── test │ ├── path_visualization.rviz │ ├── test_MeshImporter.cpp │ ├── test_ProcessPathGenerator.cpp │ ├── test_common_functions.h │ └── test_polygon_utils.cpp ├── godel_process_planning ├── CMakeLists.txt ├── config │ └── process_planning.yaml ├── include │ └── godel_process_planning │ │ └── godel_process_planning.h ├── launch │ └── process_planning.launch ├── package.xml └── src │ ├── blend_process_planning.cpp │ ├── common_utils.cpp │ ├── common_utils.h │ ├── generate_motion_plan.cpp │ ├── generate_motion_plan.h │ ├── godel_process_planning.cpp │ ├── godel_process_planning_node.cpp │ ├── keyence_process_planning.cpp │ ├── path_transitions.cpp │ ├── path_transitions.h │ ├── trajectory_utils.cpp │ └── trajectory_utils.h ├── godel_robots ├── abb │ ├── abb_file_suite │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── include │ │ │ └── abb_file_suite │ │ │ │ └── abb_motion_ftp_downloader.h │ │ ├── package.xml │ │ ├── rapid │ │ │ └── mGodel_Main.mod │ │ ├── rapid_generator │ │ │ ├── CMakeLists.txt │ │ │ ├── include │ │ │ │ └── rapid_generator │ │ │ │ │ ├── rapid_data_structures.h │ │ │ │ │ └── rapid_emitter.h │ │ │ └── src │ │ │ │ └── rapid_emitter.cpp │ │ ├── src │ │ │ ├── abb_motion_ftp_downloader.cpp │ │ │ ├── abb_motion_ftp_downloader_node.cpp │ │ │ ├── ftp_upload.cpp │ │ │ └── ftp_upload.h │ │ └── srv │ │ │ └── ExecuteProgram.srv │ ├── godel_irb1200 │ │ ├── abb_irb1200_5_90_ikfast_manipulator_plugin │ │ │ ├── CMakeLists.txt │ │ │ ├── abb_irb1200_5_90_manipulator_moveit_ikfast_plugin_description.xml │ │ │ ├── abb_irb1200_manipulator_moveit_ikfast_plugin_description.xml │ │ │ ├── include │ │ │ │ └── ikfast.h │ │ │ ├── package.xml │ │ │ ├── src │ │ │ │ ├── abb_irb1200_5_90_manipulator_ikfast_moveit_plugin.cpp │ │ │ │ ├── abb_irb1200_5_90_manipulator_ikfast_solver.cpp │ │ │ │ ├── abb_irb1200_manipulator_ikfast_moveit_plugin.cpp │ │ │ │ └── abb_irb1200_manipulator_ikfast_solver.cpp │ │ │ └── update_ikfast_plugin.sh │ │ ├── abb_irb1200_support │ │ │ ├── CMakeLists.txt │ │ │ ├── config │ │ │ │ └── joint_names_irb1200_5_90.yaml │ │ │ ├── launch │ │ │ │ ├── load_irb1200_5_90.launch │ │ │ │ ├── robot_interface_download_irb1200_5_90.launch │ │ │ │ ├── robot_state_visualize_irb1200_5_90.launch │ │ │ │ └── test_irb1200_5_90.launch │ │ │ ├── meshes │ │ │ │ └── irb1200_5_90 │ │ │ │ │ ├── collision │ │ │ │ │ ├── base_link.stl │ │ │ │ │ ├── link_1.stl │ │ │ │ │ ├── link_2.stl │ │ │ │ │ ├── link_3.stl │ │ │ │ │ ├── link_4.stl │ │ │ │ │ ├── link_5.stl │ │ │ │ │ └── link_6.stl │ │ │ │ │ └── visual │ │ │ │ │ ├── base_link.dae │ │ │ │ │ ├── link_1.dae │ │ │ │ │ ├── link_2.dae │ │ │ │ │ ├── link_3.dae │ │ │ │ │ ├── link_4.dae │ │ │ │ │ ├── link_5.dae │ │ │ │ │ └── link_6.dae │ │ │ ├── package.xml │ │ │ └── urdf │ │ │ │ ├── irb1200_5_90.xacro │ │ │ │ └── irb1200_5_90_macro.xacro │ │ ├── godel_irb1200_moveit_config │ │ │ ├── .setup_assistant │ │ │ ├── CMakeLists.txt │ │ │ ├── config │ │ │ │ ├── controllers.yaml │ │ │ │ ├── fake_controllers.yaml │ │ │ │ ├── irb1200_workspace.srdf │ │ │ │ ├── joint_limits.yaml │ │ │ │ ├── kinematics.yaml │ │ │ │ └── ompl_planning.yaml │ │ │ ├── launch │ │ │ │ ├── default_warehouse_db.launch │ │ │ │ ├── demo.launch │ │ │ │ ├── fake_moveit_controller_manager.launch.xml │ │ │ │ ├── irb1200_robot_ftp_interface.launch │ │ │ │ ├── irb1200_workspace_moveit_controller_manager.launch.xml │ │ │ │ ├── irb1200_workspace_moveit_sensor_manager.launch.xml │ │ │ │ ├── joystick_control.launch │ │ │ │ ├── move_group.launch │ │ │ │ ├── moveit.rviz │ │ │ │ ├── moveit_planning_execution.launch │ │ │ │ ├── moveit_rviz.launch │ │ │ │ ├── ompl_planning_pipeline.launch.xml │ │ │ │ ├── planning_context.launch │ │ │ │ ├── planning_pipeline.launch.xml │ │ │ │ ├── run_benchmark_ompl.launch │ │ │ │ ├── sensor_manager.launch.xml │ │ │ │ ├── setup_assistant.launch │ │ │ │ ├── trajectory_execution.launch.xml │ │ │ │ ├── warehouse.launch │ │ │ │ └── warehouse_settings.launch.xml │ │ │ └── package.xml │ │ └── godel_irb1200_support │ │ │ ├── CMakeLists.txt │ │ │ ├── config │ │ │ ├── blending_plan.yaml │ │ │ ├── plugins.yaml │ │ │ ├── point_cloud_descriptions.yaml │ │ │ ├── robot_scan.yaml │ │ │ ├── scan_plan.yaml │ │ │ └── surface_detection.yaml │ │ │ ├── launch │ │ │ └── irb1200_blending.launch │ │ │ ├── meshes │ │ │ └── visual │ │ │ │ └── automate_demo_cell_simplified.stl │ │ │ ├── package.xml │ │ │ ├── rviz │ │ │ └── irb1200_blending.rviz │ │ │ └── urdf │ │ │ ├── automate_demo_cell_macro.xacro │ │ │ └── irb1200_workspace.xacro │ └── godel_irb2400 │ │ ├── abb_irb2400_descartes │ │ ├── CMakeLists.txt │ │ ├── abb_irb2400_descartes_plugins.xml │ │ ├── include │ │ │ └── abb_irb2400_descartes │ │ │ │ └── abb_irb2400_robot_model.h │ │ ├── package.xml │ │ └── src │ │ │ └── abb_irb2400_robot_model.cpp │ │ ├── godel_irb2400_moveit_config │ │ ├── .setup_assistant │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── controllers.yaml │ │ │ ├── fake_controllers.yaml │ │ │ ├── irb2400_workspace.srdf │ │ │ ├── joint_limits.yaml │ │ │ ├── kinematics.yaml │ │ │ └── ompl_planning.yaml │ │ ├── launch │ │ │ ├── default_warehouse_db.launch │ │ │ ├── demo.launch │ │ │ ├── fake_moveit_controller_manager.launch.xml │ │ │ ├── irb2400_calib_planning_context.launch │ │ │ ├── irb2400_robot_ftp_interface.launch │ │ │ ├── irb2400_workspace_moveit_controller_manager.launch.xml │ │ │ ├── irb2400_workspace_moveit_sensor_manager.launch.xml │ │ │ ├── joystick_control.launch │ │ │ ├── move_group.launch │ │ │ ├── moveit.rviz │ │ │ ├── moveit_planning_execution.launch │ │ │ ├── moveit_rviz.launch │ │ │ ├── ompl_planning_pipeline.launch.xml │ │ │ ├── planning_context.launch │ │ │ ├── planning_pipeline.launch.xml │ │ │ ├── run_benchmark_ompl.launch │ │ │ ├── sensor_manager.launch.xml │ │ │ ├── setup_assistant.launch │ │ │ ├── trajectory_execution.launch.xml │ │ │ ├── warehouse.launch │ │ │ └── warehouse_settings.launch.xml │ │ └── package.xml │ │ ├── godel_irb2400_support │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── blending_plan.yaml │ │ │ ├── plugins.yaml │ │ │ ├── point_cloud_descriptions.yaml │ │ │ ├── robot_scan.yaml │ │ │ ├── scan_plan.yaml │ │ │ └── surface_detection.yaml │ │ ├── launch │ │ │ └── irb2400_blending.launch │ │ ├── package.xml │ │ ├── rviz │ │ │ └── irb2400_blending.rviz │ │ └── urdf │ │ │ ├── irb2400_workspace.xacro │ │ │ └── swri_demo_cell_macro.xacro │ │ └── irb2400_ikfast_manipulator_plugin │ │ ├── CMakeLists.txt │ │ ├── abb_irb2400_manipulator_moveit_ikfast_plugin_description.xml │ │ ├── include │ │ └── irb2400_ikfast_manipulator_plugin │ │ │ ├── abb_irb2400_manipulator_ikfast_moveit_plugin.hpp │ │ │ ├── abb_irb2400_manipulator_ikfast_solver.hpp │ │ │ └── ikfast.h │ │ ├── package.xml │ │ ├── src │ │ └── plugin_init.cpp │ │ └── update_ikfast_plugin.sh └── blending_end_effector │ ├── CMakeLists.txt │ ├── config │ └── robot_state_visualize.rviz │ ├── launch │ └── blending_end_effector.launch │ ├── meshes │ ├── collision │ │ ├── EE_and_Tool_Changer_convex_hull.STL │ │ └── adapter_tube_convex_hull.STL │ └── visual │ │ ├── Adapter_Tube.STL │ │ ├── Blending_Tool.STL │ │ ├── Ensenso.STL │ │ ├── Laser_Sensor.STL │ │ └── Tool_Mounting_Plate.STL │ ├── package.xml │ └── urdf │ ├── blending_eef.urdf │ └── blending_eff_macro.xacro ├── godel_scan_analysis ├── CMakeLists.txt ├── include │ └── godel_scan_analysis │ │ ├── keyence_scan_server.h │ │ ├── scan_algorithms.h │ │ ├── scan_roughness_scoring.h │ │ └── scan_utilities.h ├── launch │ └── scan_analysis.launch ├── package.xml └── src │ ├── godel_scan_analysis_node.cpp │ ├── keyence_scan_server.cpp │ └── scan_roughness_scoring.cpp ├── godel_simple_gui ├── CMakeLists.txt ├── include │ └── godel_simple_gui │ │ ├── blending_panel.h │ │ ├── blending_widget.h │ │ ├── gui_state.h │ │ ├── options │ │ ├── path_planning_configuration.h │ │ ├── pose_widget.h │ │ ├── robot_scan_configuration.h │ │ ├── scan_tool_configuration.h │ │ └── surface_detection_configuration.h │ │ ├── options_submenu.h │ │ ├── parameter_window_base.h │ │ └── states │ │ ├── error_state.h │ │ ├── executing_state.h │ │ ├── planning_state.h │ │ ├── scan_teach_state.h │ │ ├── scanning_state.h │ │ ├── select_all_surface_state.h │ │ ├── select_plans_state.h │ │ ├── simulating_state.h │ │ ├── surface_select_state.h │ │ └── wait_to_execute_state.h ├── package.xml ├── plugin.xml └── src │ ├── blending_panel.cpp │ ├── blending_widget.cpp │ ├── options │ ├── path_planning_configuration.cpp │ ├── pose_widget.cpp │ ├── robot_scan_configuration.cpp │ ├── scan_tool_configuration.cpp │ └── surface_detection_configuration.cpp │ ├── options_submenu.cpp │ ├── parameter_window_base.cpp │ ├── states │ ├── error_state.cpp │ ├── executing_state.cpp │ ├── planning_state.cpp │ ├── scan_teach_state.cpp │ ├── scanning_state.cpp │ ├── select_all_surface_state.cpp │ ├── select_plans_state.cpp │ ├── simulating_state.cpp │ ├── surface_select_state.cpp │ └── wait_to_execute_state.cpp │ └── uis │ ├── blending_widget.ui │ ├── options_select_menu.ui │ ├── path_planning_configuration.ui │ ├── pose_widget.ui │ ├── robot_scan_configuration.ui │ ├── scan_tool_configuration.ui │ └── surface_detection_configuration.ui ├── godel_surface_detection ├── CMakeLists.txt ├── include │ ├── coordination │ │ └── data_coordinator.h │ ├── detection │ │ └── surface_detection.h │ ├── interactive │ │ └── interactive_surface_server.h │ ├── scan │ │ └── robot_scan.h │ ├── segmentation │ │ └── surface_segmentation.h │ ├── services │ │ ├── surface_blending_service.h │ │ └── trajectory_library.h │ └── utils │ │ └── mesh_conversions.h ├── launch │ └── godel_core.launch ├── package.xml └── src │ ├── coordination │ └── data_coordinator.cpp │ ├── detection │ └── surface_detection.cpp │ ├── interactive │ └── interactive_surface_server.cpp │ ├── nodes │ ├── boundary_test_node.cpp │ ├── point_cloud_generator_node.cpp │ └── point_cloud_publisher_node.cpp │ ├── scan │ └── robot_scan.cpp │ ├── segmentation │ ├── parallel_boundary.h │ ├── parallel_boundary.hpp │ └── surface_segmentation.cpp │ ├── services │ ├── blending_service_path_generation.cpp │ ├── surface_blending_service.cpp │ └── trajectory_library.cpp │ └── utils │ └── mesh_conversions.cpp ├── godel_utils ├── CMakeLists.txt ├── include │ └── godel_utils │ │ └── ensenso_guard.h ├── package.xml └── src │ └── ensenso_guard.cpp ├── industrial_robot_simulator_service ├── CMakeLists.txt ├── launch │ └── start_simulation_server.launch ├── package.xml ├── src │ └── simulator_service_node.cpp └── srv │ └── SimulateTrajectory.srv ├── meshing_plugins ├── CMakeLists.txt ├── config │ └── meshing.yaml ├── include │ └── meshing_plugins │ │ └── concave_hull_plugins.h ├── package.xml ├── plugin.xml └── src │ └── concave_hull_mesher.cpp ├── meshing_plugins_base ├── CMakeLists.txt ├── include │ └── meshing_plugins_base │ │ └── meshing_base.h └── package.xml ├── path_planning_plugins ├── CMakeLists.txt ├── config │ └── path_planning.yaml ├── include │ ├── mesh_importer │ │ └── mesh_importer.h │ ├── path_planning_plugins │ │ └── openveronoi_plugins.h │ └── profilometer │ │ └── profilometer_scan.h ├── package.xml ├── plugin.xml └── src │ ├── mesh_importer │ └── mesh_importer.cpp │ ├── openveronoi │ ├── blend_planner.cpp │ └── scan_planner.cpp │ └── profilometer │ └── profilometer_scan.cpp └── path_planning_plugins_base ├── CMakeLists.txt ├── include └── path_planning_plugins_base │ └── path_planning_base.h └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Temporary files 2 | *~ 3 | # Compiled Object files 4 | *.slo 5 | *.lo 6 | *.o 7 | 8 | # Compiled Dynamic libraries 9 | *.so 10 | *.dylib 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | 17 | # Cmake/catkin files 18 | CMakeFiles 19 | Makefile 20 | catkin_generated 21 | *.cmake 22 | CMakeCache.txt 23 | devel 24 | 25 | # Eclipse files 26 | .project 27 | .cproject 28 | 29 | # mongodb instance 30 | default_warehouse_mongo_db 31 | 32 | # editor files 33 | *.autosave -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | notifications: 7 | email: 8 | on_success: change 9 | on_failure: always 10 | env: 11 | global: 12 | - ROS_DISTRO=kinetic 13 | - UPSTREAM_WORKSPACE=file 14 | - ROSINSTALL_FILENAME=godel.rosinstall 15 | - NOT_TEST_INSTALL=true 16 | - BEFORE_SCRIPT='catkin config -w $CATKIN_WORKSPACE --no-install' 17 | matrix: 18 | - ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 19 | - ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 20 | install: 21 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 22 | script: 23 | - source .ci_config/travis.sh 24 | -------------------------------------------------------------------------------- /godel.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: godel_openvoronoi, uri: 'https://github.com/ros-industrial-consortium/godel_openvoronoi.git', version: hydro-devel} 2 | - git: {local-name: godel, uri: 'https://github.com/ros-industrial-consortium/godel.git', version: kinetic-devel} 3 | - git: {local-name: industrial_core, uri: 'https://github.com/ros-industrial/industrial_core.git', version: kinetic-devel} 4 | - git: {local-name: keyence_experimental, uri: 'https://github.com/ros-industrial/keyence_experimental.git', version: kinetic-devel} 5 | - git: {local-name: libsocket, uri: 'https://github.com/dermesser/libsocket.git', version: master} 6 | - git: {local-name: swri_profiler, uri: 'https://github.com/swri-robotics/swri_profiler.git', version: master} 7 | 8 | # Custom abb until Kinetic release happens 9 | - git: {local-name: abb, uri: 'https://github.com/ros-industrial/abb.git', version: kinetic-devel} 10 | # Custom descartes until we get changes merged in 11 | - git: {local-name: descartes, uri: 'https://github.com/Jmeyer1292/descartes.git', version: godel_refactor_omp} 12 | -------------------------------------------------------------------------------- /godel/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /godel/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel 4 | 0.0.0 5 | The godel metapackage 6 | 7 | ros-industrial 8 | 9 | Apache2 10 | catkin 11 | 12 | godel_plugins 13 | godel_process_planning 14 | godel_process_path_generation 15 | godel_msgs 16 | godel_surface_detection 17 | godel_polygon_offset 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_msgs/action/BlendProcessExecution.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | trajectory_msgs/JointTrajectory trajectory_approach 3 | trajectory_msgs/JointTrajectory trajectory_process 4 | trajectory_msgs/JointTrajectory trajectory_depart 5 | 6 | bool wait_for_execution 7 | 8 | bool simulate 9 | 10 | godel_msgs/BlendingPlanParameters params 11 | 12 | --- 13 | 14 | # Result 15 | bool success 16 | 17 | --- 18 | 19 | # Feedback is empty (completion of task is our main concern) 20 | -------------------------------------------------------------------------------- /godel_msgs/action/PathPlanning.action: -------------------------------------------------------------------------------- 1 | # Service used to generate cartesian path of single surface. 2 | 3 | # Goal 4 | godel_msgs/PathPlanningParameters params 5 | godel_msgs/SurfaceBoundaries surface 6 | --- 7 | # Result 8 | geometry_msgs/PoseArray poses 9 | --- 10 | # Feedback 11 | string last_completed 12 | -------------------------------------------------------------------------------- /godel_msgs/action/ProcessExecution.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | 3 | # Joint trajectory 4 | trajectory_msgs/JointTrajectory trajectory_approach 5 | trajectory_msgs/JointTrajectory trajectory_process 6 | trajectory_msgs/JointTrajectory trajectory_depart 7 | 8 | # wait for execution to finish before returning 9 | bool wait_for_execution 10 | 11 | # Use the simulated robot or the actual robot 12 | bool simulate 13 | 14 | # Process parameters 15 | godel_msgs/BlendingPlanParameters blend_params 16 | godel_msgs/ScanPlanParameters scan_params 17 | 18 | --- 19 | # Result 20 | 21 | bool success 22 | 23 | --- 24 | # Feedback is empty (primary concern is that task completes) 25 | -------------------------------------------------------------------------------- /godel_msgs/action/ProcessPlanning.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | int32 GENERATE_MOTION_PLAN_AND_PREVIEW=1 3 | int32 PREVIEW_TOOL_PATH=2 4 | 5 | int32 action 6 | 7 | bool use_default_parameters 8 | godel_msgs/PathPlanningParameters params 9 | godel_msgs/SurfaceBoundaries surface 10 | 11 | --- 12 | 13 | # Result 14 | bool succeeded 15 | 16 | --- 17 | 18 | # Feedback 19 | string last_completed 20 | -------------------------------------------------------------------------------- /godel_msgs/action/SelectMotionPlan.action: -------------------------------------------------------------------------------- 1 | # Select Motion Plan Action 2 | 3 | # Goal 4 | 5 | # Used by external processes to command the 'godel_surface_blending' service to 6 | # execute an 'available' motion plan. 7 | # See 'LoadSaveMotionPlan.srv' for more details on the motion plan concept 8 | 9 | string name # Name of the motion plan to be executed 10 | 11 | bool simulate # If true, the motion plan will be simulated and not executed. 12 | 13 | bool wait_for_execution # If true, the execution service will block until the trajectory 14 | # is complete. Otherwise, returns immediately. 15 | --- 16 | # Result 17 | 18 | # Return Code Enumeration (Negative values for error types) 19 | int32 SUCCESS=1 20 | int32 NO_SUCH_NAME=-1 21 | int32 TIMEOUT=-2 22 | 23 | # Error code (see above enum) indicating ability to START the motion plan. 24 | # This error code does not currently capture all of the things that might 25 | # go wrong during the execution of such a plan. 26 | int32 code 27 | 28 | --- 29 | # Feedback is empty 30 | -------------------------------------------------------------------------------- /godel_msgs/msg/BlendingPlanParameters.msg: -------------------------------------------------------------------------------- 1 | # Path Parameters 2 | float64 tool_radius # (m) actual radius of blending tool 3 | float64 margin # (m) additional distance to leave near surface boundaries 4 | float64 overlap # (m) overlap distance between adjacent paths 5 | 6 | # Process Parameters 7 | float64 spindle_speed # (rpm) Spindle Speed 8 | float64 tool_force # (kg) Downward force to apply 9 | 10 | # Path Speeds 11 | float64 approach_spd # (m/s) Speed to approach surface of part 12 | float64 blending_spd # (m/s) Speed to perform blending 13 | float64 retract_spd # (m/s) Speed to move away from surface of part 14 | float64 traverse_spd # (m/s) Speed to travel while at traverse height 15 | 16 | # Misc 17 | float64 discretization # (m) How densely to space adjacent Cartesian points 18 | float64 safe_traverse_height # (m) height above surface to rise during rapid traversal moves 19 | float64 z_adjust # (m) height adjustment along surface normal to adjust for different tools 20 | 21 | # Pre-Process Surface Parameters 22 | float64 min_boundary_length # (m) Boundaries below threshold are ignored during process path creation 23 | -------------------------------------------------------------------------------- /godel_msgs/msg/PathPlanningParameters.msg: -------------------------------------------------------------------------------- 1 | # Plan Parameters 2 | float64 discretization # (m) How densely to space adjacent Cartesian points 3 | float64 margin # (m) additional distance to leave near surface boundaries 4 | float64 overlap # (m) overlap distance between adjacent paths 5 | float64 scan_width # (m) width of scanner's beam 6 | float64 tool_radius # (m) radius of effector tool (e.g. blending pad) 7 | float64 traverse_height # (m) The minimum distance (z-axis) required to safely traverse 8 | -------------------------------------------------------------------------------- /godel_msgs/msg/ProcessPath.msg: -------------------------------------------------------------------------------- 1 | # ProcessPath 2 | 3 | # The frame of reference for poses is assumed to be the root of the 4 | # workspace as specified in the URDF 5 | 6 | # The process path consists of some number of 'pose sequences' each of which 7 | # represents a segment of the path in which the tool constraints must be held. 8 | # In between segments, the tool can 'retract' and the robot can change config. 9 | geometry_msgs/PoseArray[] segments 10 | -------------------------------------------------------------------------------- /godel_msgs/msg/ProcessPlan.msg: -------------------------------------------------------------------------------- 1 | # Godel supports two processes at the moment: surface blending and line scanning 2 | # This msg encapsulates a motion plan for one of these components. 3 | 4 | # Enumeration represeting plan type 5 | int32 BLEND_TYPE=0 6 | int32 SCAN_TYPE=1 7 | 8 | # The plan is broken into components so that nodes responsible 9 | # for the actual execution can perform any necessary IO commands 10 | # as the robot begins and completes the actual process path. In 11 | # our case, the blending tool and laser scanner turn on after the 12 | # approach and off after the process path is done. 13 | trajectory_msgs/JointTrajectory trajectory_approach 14 | trajectory_msgs/JointTrajectory trajectory_process 15 | trajectory_msgs/JointTrajectory trajectory_depart 16 | int32 type 17 | -------------------------------------------------------------------------------- /godel_msgs/msg/RobotScanParameters.msg: -------------------------------------------------------------------------------- 1 | # path planning 2 | string group_name 3 | string home_position 4 | string world_frame 5 | string tcp_frame 6 | 7 | # trajectory generation 8 | geometry_msgs/Pose tcp_to_cam_pose 9 | geometry_msgs/Pose world_to_obj_pose 10 | float64 cam_to_obj_zoffset 11 | float64 cam_to_obj_xoffset 12 | float64 cam_tilt_angle # rotation about relative object's y axis (radians) 13 | float64 sweep_angle_start 14 | float64 sweep_angle_end 15 | int32 num_scan_points 16 | float64 reachable_scan_points_ratio 17 | bool stop_on_planning_error 18 | 19 | # scan data source 20 | string scan_topic 21 | string scan_target_frame 22 | -------------------------------------------------------------------------------- /godel_msgs/msg/ScanPlanParameters.msg: -------------------------------------------------------------------------------- 1 | # Path Parameters 2 | float64 scan_width # (m) actual width of scanner's beam 3 | float64 margin # (m) additional distance to leave near surface boundaries 4 | float64 overlap # (m) overlap distance between adjacent paths 5 | float64 approach_distance # (m) approach distance 6 | float64 traverse_spd # (m/s) Speed to travel while at traverse height 7 | 8 | # Misc 9 | float64 z_adjust # (m) height adjustment along surface normal to adjust for different tools 10 | 11 | # QA Methods 12 | int32 METHOD_RMS=0 13 | 14 | # QA Parameters 15 | # These values, combined with your choice of scoring metric, are used to colorize 16 | # points in individual line scans. 'out-of-spec' means that the metric (say RMS) over 17 | # the 'window_width' exceeds the 'max_qa_value'. In this case, the associated point 18 | # will be colored bright red. The 'min_qa_value' field is used to scale the color 19 | # of points with scores in between the min and max values. 20 | int32 quality_metric # line scanner scoring metric (see QA Methods enumeration) 21 | float64 window_width # (m) width of window to score scan 22 | float64 min_qa_value # minimum value (units are function of QA method) 23 | float64 max_qa_value # max value (units are function of QA method) 24 | -------------------------------------------------------------------------------- /godel_msgs/msg/SelectedSurfacesChanged.msg: -------------------------------------------------------------------------------- 1 | string[] selected_surfaces 2 | -------------------------------------------------------------------------------- /godel_msgs/msg/SurfaceBoundaries.msg: -------------------------------------------------------------------------------- 1 | # Boundary Data for a given blending surface (e.g. pocket) 2 | # Sent as a series of 2D polygons (z-component is ignored) on XY plane of local frame. 3 | # CCW polygons are external boundaries, CW polygons internal "obstacles" 4 | geometry_msgs/Polygon[] boundaries 5 | 6 | # Local frame of surface relative to planning frame. boundaries are expressed in this local frame. 7 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /godel_msgs/msg/SurfaceDetectionParameters.msg: -------------------------------------------------------------------------------- 1 | # acquisition 2 | string frame_id 3 | 4 | # filter and normal estimation 5 | int32 meanK 6 | int32 k_search 7 | float64 stdv_threshold 8 | 9 | # region growing 10 | int32 rg_min_cluster_size 11 | int32 rg_max_cluster_size 12 | int32 rg_neightbors 13 | float64 rg_smoothness_threshold 14 | float64 rg_curvature_threshold 15 | 16 | # fast triangulation 17 | float64 tr_search_radius 18 | float64 tr_mu 19 | int32 tr_max_nearest_neighbors 20 | float64 tr_max_surface_angle 21 | float64 tr_min_angle 22 | float64 tr_max_angle 23 | bool tr_normal_consistency 24 | 25 | # plane approximation refinement: Assumes planar surfaces in order to identify additional inlier points 26 | bool pa_enabled 27 | int32 pa_seg_max_iterations 28 | float64 pa_seg_dist_threshold 29 | float64 pa_sac_plane_distance 30 | float64 pa_kdtree_radius 31 | 32 | # voxel downsampling 33 | float64 voxel_leafsize 34 | 35 | # octomap occupancy 36 | bool use_octomap 37 | float64 occupancy_threshold 38 | 39 | # moving least square smoothing 40 | float64 mls_upsampling_radius 41 | float64 mls_search_radius 42 | int32 mls_point_density 43 | 44 | # tabletop segmentation 45 | bool use_tabletop_seg 46 | float64 tabletop_seg_distance_threshold 47 | 48 | # options 49 | float64 marker_alpha 50 | bool ignore_largest_cluster 51 | -------------------------------------------------------------------------------- /godel_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_msgs 4 | 0.0.0 5 | The godel_msgs package 6 | 7 | jnicho 8 | Apache2 9 | 10 | catkin 11 | actionlib 12 | actionlib_msgs 13 | genmsg 14 | geometry_msgs 15 | message_runtime 16 | sensor_msgs 17 | trajectory_msgs 18 | visualization_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /godel_msgs/srv/BlendProcessPlanning.srv: -------------------------------------------------------------------------------- 1 | # Blend process parameters 2 | godel_msgs/BlendingPlanParameters params 3 | 4 | # The actual path the tool will follow 5 | godel_msgs/ProcessPath path 6 | 7 | --- 8 | 9 | godel_msgs/ProcessPlan plan -------------------------------------------------------------------------------- /godel_msgs/srv/BlendingPlan.srv: -------------------------------------------------------------------------------- 1 | # Blending Plan Service 2 | # Called by Surface Detection and serviced by Path Planning node 3 | 4 | # Parameters defining path planning process 5 | godel_msgs/BlendingPlanParameters params 6 | 7 | # Series of surfaces to plan for. 8 | # Each surface is planned individually, then plans are linked by joint-moves. 9 | godel_msgs/SurfaceBoundaries[] surfaces 10 | --- 11 | 12 | # Resultant Joint Trajectory to process entire Blending Plan 13 | trajectory_msgs/JointTrajectory trajectory 14 | -------------------------------------------------------------------------------- /godel_msgs/srv/EnsensoCommand.srv: -------------------------------------------------------------------------------- 1 | # EnsensoCommand 2 | # Called to manage the ensenso_publisher node (e.g. to start or stop the ensenso) 3 | int32 STOP=0 4 | int32 START=1 5 | 6 | int32 action 7 | --- 8 | bool result 9 | -------------------------------------------------------------------------------- /godel_msgs/srv/GetAvailableMotionPlans.srv: -------------------------------------------------------------------------------- 1 | # empty request 2 | --- 3 | string[] names -------------------------------------------------------------------------------- /godel_msgs/srv/KeyenceProcessPlanning.srv: -------------------------------------------------------------------------------- 1 | # Blend process parameters 2 | godel_msgs/ScanPlanParameters params 3 | 4 | # The actual path the tool will follow 5 | godel_msgs/ProcessPath path 6 | 7 | --- 8 | 9 | godel_msgs/ProcessPlan plan 10 | -------------------------------------------------------------------------------- /godel_msgs/srv/LoadSaveMotionPlan.srv: -------------------------------------------------------------------------------- 1 | # Load-Save Motion Plan Service 2 | # Used by GUI components to instruct the primary blending service to perform IO related 3 | # to motion plans. Motion plans are stored to disk as bag-files through a special interface 4 | # in 'godel_surface_blending'. A motion plan contains a map of plan names to motion plans, 5 | # which include joint trajectories, type, and other context. Once loaded, the motion plans 6 | # become 'available'. Available plans may be seen through the 'GetAvailableMotionPlans.srv' 7 | # and executed through the 'SelectMotionPlan.srv'. If 'MODE_SAVE' is set, all available 8 | # motion plans are saved to the given 'path'. 9 | 10 | # I/O Mode Enumeration 11 | int32 MODE_LOAD=0 12 | int32 MODE_SAVE=1 13 | 14 | int32 mode # Read or write (see I/O Mode Enumeration) 15 | string path # Location to read from or write to depending on 'mode' 16 | --- 17 | 18 | # Return Codes 19 | int32 SUCCESS=0 # No issue reading/writing 20 | int32 NO_SUCH_FILE=1 # For 'MODE_LOAD'; File could not be found. 21 | int32 ERROR_LOADING=2 # For 'MODE_LOAD'; File could not be read. 22 | int32 ERROR_WRITING=3 # For 'MODE_SAVE'; File could not be written. 23 | 24 | int32 code # Success code, see Return Code enumeration 25 | -------------------------------------------------------------------------------- /godel_msgs/srv/OffsetBoundary.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Polygon[] polygons # Boundary order CCW external, CW internal 2 | float64 offset_distance # Distance (m) to perform typical offset 3 | float64 initial_offset # If specified (>0), distance (m) to perform initial offset 4 | float64 discretization # Max Distance (m) used to discretize lines/arcs 5 | --- 6 | geometry_msgs/Polygon[] offset_polygons # Ordered list of offset polygons 7 | float64[] offsets # List of distances, each offset corresponds to offset_boundary depth 8 | -------------------------------------------------------------------------------- /godel_msgs/srv/PathPlanning.srv: -------------------------------------------------------------------------------- 1 | # Service used to generate cartesian path of single surface. 2 | 3 | godel_msgs/PathPlanningParameters params 4 | godel_msgs/SurfaceBoundaries surface 5 | --- 6 | geometry_msgs/PoseArray poses 7 | duration[] sleep_times 8 | -------------------------------------------------------------------------------- /godel_msgs/srv/ProcessPlanning.srv: -------------------------------------------------------------------------------- 1 | int32 GENERATE_MOTION_PLAN=1 2 | int32 GENERATE_MOTION_PLAN_AND_PREVIEW=2 3 | int32 PREVIEW_TOOL_PATH=3 4 | int32 PREVIEW_MOTION_PLAN=4 5 | int32 EXECUTE_MOTION_PLAN=5 6 | 7 | int32 action 8 | 9 | bool use_default_parameters 10 | godel_msgs/PathPlanningParameters params 11 | godel_msgs/SurfaceBoundaries surface 12 | 13 | --- 14 | 15 | bool succeeded 16 | 17 | -------------------------------------------------------------------------------- /godel_msgs/srv/RenameSurface.srv: -------------------------------------------------------------------------------- 1 | # Used to rename a surface loaded into the surface server 2 | string old_name 3 | string new_name 4 | 5 | --- 6 | int32 SUCCESS=0 7 | int32 NO_SUCH_NAME=1 8 | 9 | int32 code 10 | -------------------------------------------------------------------------------- /godel_msgs/srv/SelectMotionPlan.srv: -------------------------------------------------------------------------------- 1 | # Select Motion Plan Service 2 | # Used by external processes to command the 'godel_surface_blending' service to 3 | # execute an 'available' motion plan. 4 | 5 | # See 'LoadSaveMotionPlan.srv' for more details on the motion plan concept 6 | 7 | string name # Name of the motion plan to be executed 8 | 9 | bool simulate # If true, the motion plan will be simulated and not executed. 10 | 11 | bool wait_for_execution # If true, the execution service will block until the trajectory 12 | # is complete. Otherwise, returns immediately. 13 | --- 14 | # Return Code Enumeration 15 | int32 SUCCESS=0 16 | int32 NO_SUCH_NAME=1 17 | 18 | # Error code (see above enum) indicating ability to START the motion plan. 19 | # This error code does not currently capture all of the things that might 20 | # go wrong during the execution of such a plan. 21 | int32 code -------------------------------------------------------------------------------- /godel_msgs/srv/SelectSurface.srv: -------------------------------------------------------------------------------- 1 | int32 SELECT=0 2 | int32 DESELECT=1 3 | int32 SELECT_ALL=2 4 | int32 DESELECT_ALL=3 5 | int32 HIDE_ALL=4 6 | int32 SHOW_ALL=5 7 | int32 action 8 | 9 | string[] select_surfaces 10 | --- 11 | bool succeeded 12 | -------------------------------------------------------------------------------- /godel_msgs/srv/SurfaceBlendingParameters.srv: -------------------------------------------------------------------------------- 1 | # Surface Blending Parameter Getter/Setter 2 | # This service is used to update/query the internal state of the 3 | # 'surface_blending_service'. This is means by which GUI components 4 | # may visualize and mutate the state of various system parameters. 5 | 6 | # Action Enumeration 7 | int32 GET_CURRENT_PARAMETERS=0 # Get the base parameter state 8 | int32 GET_DEFAULT_PARAMETERS=1 # Get the parameters loaded at start-up 9 | int32 SET_PARAMETERS=2 # Set the state of the system 10 | int32 SAVE_PARAMETERS=3 # Both sets the state and caches the new state to disk 11 | 12 | int32 action # Action to undertake (see Action Enumeration) 13 | 14 | # In the case that action is a GET method, then these fields may be left blank 15 | godel_msgs/RobotScanParameters robot_scan # Parameters associated with macro-scans 16 | godel_msgs/SurfaceDetectionParameters surface_detection # Surface detection/extraction parameters 17 | godel_msgs/BlendingPlanParameters blending_plan # Blending parameters 18 | godel_msgs/ScanPlanParameters scan_plan # Surface-scanning and QA parameters 19 | godel_msgs/PathPlanningParameters path_params # Generic path planning parameters 20 | 21 | --- 22 | # The success of the GET/SET method. GET should always succeed. SET may fail if parameters 23 | # are out of bounds or a save location is not writable. 24 | bool succeeded 25 | 26 | # If action in request was a SET method, then these fields are not populated. 27 | godel_msgs/RobotScanParameters robot_scan 28 | godel_msgs/SurfaceDetectionParameters surface_detection 29 | godel_msgs/BlendingPlanParameters blending_plan 30 | godel_msgs/ScanPlanParameters scan_plan 31 | godel_msgs/PathPlanningParameters path_params 32 | -------------------------------------------------------------------------------- /godel_msgs/srv/SurfaceDetection.srv: -------------------------------------------------------------------------------- 1 | int32 PUBLISH_SCAN_PATH=2 2 | int32 SCAN_AND_FIND_ONLY=3 3 | int32 SCAN_FIND_AND_RETURN=4 4 | int32 RETURN_LATEST_RESULTS=5 5 | int32 FIND_ONLY=6 6 | int32 FIND_AND_RETURN=7 7 | int32 INITIALIZE_SPACE=8 8 | int32 VISUALIZATION_REQUEST=9 9 | int32 action 10 | bool use_default_parameters 11 | godel_msgs/RobotScanParameters robot_scan 12 | godel_msgs/SurfaceDetectionParameters surface_detection 13 | string name 14 | 15 | --- 16 | bool surfaces_found 17 | visualization_msgs/MarkerArray surfaces 18 | geometry_msgs/PoseArray robot_scan_poses 19 | godel_msgs/RobotScanParameters robot_scan # parameter values used in latests results 20 | godel_msgs/SurfaceDetectionParameters surface_detection # parameter values used in latests results 21 | -------------------------------------------------------------------------------- /godel_msgs/srv/TrajectoryExecution.srv: -------------------------------------------------------------------------------- 1 | # Trajectory Execution Service 2 | # This service executes/simulates a single continuous joint motion. 3 | # This service was added in combination with 'godel_path_execution' in order 4 | # to centralize the duty of managing robot motion. 5 | 6 | # Joint trajectory 7 | trajectory_msgs/JointTrajectory trajectory 8 | 9 | # wait for execution to finish before returning 10 | bool wait_for_execution 11 | 12 | # Use the simulated robot or the actual robot 13 | bool simulate 14 | --- 15 | # Empty 16 | -------------------------------------------------------------------------------- /godel_msgs/srv/TrajectoryPlanning.srv: -------------------------------------------------------------------------------- 1 | # Trajectory Planning Service 2 | # This service makes a request to the centralized path planner 3 | # to calculate a robot joint trajectory that achieves the 4 | # plan set out in 'path'. 5 | 6 | # The cartesian points representing path to be processed 7 | ProcessPath path 8 | 9 | # MoveIt group name associated with this plan (see the associated SRDF) 10 | string group_name 11 | 12 | # Frames of reference for planning 13 | string tool_frame 14 | string world_frame 15 | 16 | # Process type; True for blending path; False for line scanner pass 17 | # Dictates the constraints put on the tool. 18 | bool is_blending_path 19 | 20 | # If true, the planner will attempt to create a joint motion from the 21 | # 'current' robot joint state to the first point in the process path. 22 | bool plan_from_current_position 23 | 24 | # How finely to search unconstrained angles, if any 25 | float64 angle_discretization 26 | 27 | # Timing data 28 | float64 tcp_speed # Linear tool speed (m/s) 29 | 30 | --- 31 | 32 | # Resulting trajectory components 33 | trajectory_msgs/JointTrajectory trajectory_approach 34 | trajectory_msgs/JointTrajectory trajectory_process 35 | trajectory_msgs/JointTrajectory trajectory_depart 36 | 37 | -------------------------------------------------------------------------------- /godel_param_helpers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_param_helpers) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | ) 7 | 8 | catkin_package( 9 | INCLUDE_DIRS include 10 | CATKIN_DEPENDS roscpp 11 | ) 12 | 13 | include_directories( 14 | include 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | install(DIRECTORY include/${PROJECT_NAME}/ 19 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 20 | FILES_MATCHING PATTERN "*.h" 21 | PATTERN ".svn" EXCLUDE 22 | ) 23 | -------------------------------------------------------------------------------- /godel_param_helpers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_param_helpers 4 | 0.1.0 5 | 6 | This package implements routines for loading, saving, and checking for the 7 | existence of user parameters. 8 | 9 | 10 | Jonathan Meyer 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | roscpp 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_path_execution/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_path_execution) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | godel_msgs 6 | roscpp 7 | industrial_robot_simulator_service 8 | actionlib 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS 13 | actionlib 14 | godel_msgs 15 | roscpp 16 | industrial_robot_simulator_service 17 | ) 18 | 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ) 23 | 24 | add_executable(path_execution_service_node 25 | src/path_execution_service_node.cpp 26 | src/path_execution_service.cpp 27 | ) 28 | 29 | add_dependencies(path_execution_service_node godel_msgs_generate_messages_cpp) 30 | add_dependencies(path_execution_service_node industrial_robot_simulator_service_generate_messages_cpp) 31 | 32 | target_link_libraries(path_execution_service_node 33 | ${catkin_LIBRARIES} 34 | ) 35 | 36 | install(TARGETS path_execution_service_node 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 40 | ) 41 | 42 | install(DIRECTORY launch 43 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 44 | ) 45 | -------------------------------------------------------------------------------- /godel_path_execution/include/godel_path_execution/path_execution_service.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_EXECUTION_SERVICE_H 2 | #define PATH_EXECUTION_SERVICE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace godel_path_execution 10 | { 11 | 12 | class PathExecutionService 13 | { 14 | public: 15 | PathExecutionService(ros::NodeHandle& nh); 16 | 17 | /** 18 | * Currently forwards the godel_msgs::TrajectoryExecution on to the corresponding 19 | * MoveIt node. The idea though is that abstracting 'execution' will give us more flexibility 20 | * later to implement our own process parameters related to execution. 21 | */ 22 | bool executionCallback(godel_msgs::TrajectoryExecution::Request& req, 23 | godel_msgs::TrajectoryExecution::Response& res); 24 | 25 | private: 26 | ros::ServiceServer server_; 27 | actionlib::SimpleActionClient ac_; 28 | std::string name_; 29 | }; 30 | } 31 | 32 | #endif // PATH_EXECUTION_SERVICE_H 33 | -------------------------------------------------------------------------------- /godel_path_execution/launch/path_execution_service.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /godel_path_execution/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_path_execution 4 | 0.1.0 5 | 6 | This package manages the execution of joint trajectories 7 | on the physical platform. Many components in Godel may 8 | issue movement commands, and this package is envisioned 9 | as a gate keeper of this motion. 10 | 11 | 12 | Jonathan Meyer 13 | Apache 2.0 14 | 15 | catkin 16 | 17 | actionlib 18 | godel_msgs 19 | roscpp 20 | industrial_robot_simulator_service 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_path_execution/src/path_execution_service_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "path_execution_service_node"); 8 | 9 | ros::NodeHandle nh; 10 | 11 | godel_path_execution::PathExecutionService path_executor(nh); 12 | 13 | ros::spin(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /godel_plugins/include/godel_plugins/rviz_panels/robot_blending_panel.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright Mar 13, 2014 Southwest Research Institute 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | */ 16 | 17 | #ifndef ROBOT_BLENDING_PANEL_H_ 18 | #define ROBOT_BLENDING_PANEL_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace godel_plugins 25 | { 26 | namespace rviz_panels 27 | { 28 | 29 | class RobotBlendingPanel : public rviz::Panel 30 | { 31 | Q_OBJECT 32 | public: 33 | RobotBlendingPanel(QWidget* parent = 0); 34 | virtual ~RobotBlendingPanel(); 35 | 36 | virtual void onInitialize(); 37 | 38 | protected Q_SLOTS: 39 | 40 | // rviz::Panel virtual functions 41 | virtual void load(const rviz::Config& config); 42 | virtual void save(rviz::Config config) const; 43 | 44 | protected: 45 | QWidget* widget_; 46 | }; 47 | 48 | } /* namespace rviz_panels */ 49 | } /* namespace godel_plugins */ 50 | #endif /* ROBOT_BLENDING_PANEL_H_ */ 51 | -------------------------------------------------------------------------------- /godel_plugins/include/godel_plugins/widgets/parameter_window_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETER_WINDOW_BASE_H 2 | #define PARAMETER_WINDOW_BASE_H 3 | 4 | #include 5 | 6 | namespace godel_plugins 7 | { 8 | 9 | /** 10 | * @brief Abstract base class for parameter windows in the blending widget 11 | */ 12 | class ParameterWindowBase : public QMainWindow 13 | { 14 | Q_OBJECT 15 | 16 | public: 17 | /** 18 | * @brief Updates GUI parameters and shows the main window 19 | */ 20 | virtual void show(); 21 | 22 | Q_SIGNALS: 23 | void parameters_changed(); 24 | void parameters_save_requested(); 25 | 26 | protected: 27 | /** 28 | * @brief Reads the internal data structure to update the fields of the GUI 29 | */ 30 | virtual void update_gui_fields() = 0; 31 | /** 32 | * @brief Reads the fields of the GUI to update the internal data structure 33 | */ 34 | virtual void update_internal_values() = 0; 35 | 36 | protected Q_SLOTS: 37 | 38 | virtual void accept_changes_handler(); 39 | virtual void cancel_changes_handler(); 40 | virtual void save_changes_handler(); 41 | }; 42 | } 43 | 44 | #endif // PARAMETER_WINDOW_BASE_H 45 | -------------------------------------------------------------------------------- /godel_plugins/include/godel_plugins/widgets/path_planning_param_window.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_PLANNING_PARAM_WINDOW_H 2 | #define PATH_PLANNING_PARAM_WINDOW_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_plugins 9 | { 10 | 11 | class PathPlanningConfigWidget : public ParameterWindowBase 12 | { 13 | Q_OBJECT 14 | public: 15 | PathPlanningConfigWidget(const godel_msgs::PathPlanningParameters& params); 16 | 17 | godel_msgs::PathPlanningParameters& params() { return params_; } 18 | 19 | protected: 20 | virtual void update_gui_fields(); 21 | 22 | virtual void update_internal_values(); 23 | 24 | godel_msgs::PathPlanningParameters params_; 25 | Ui::PathPlanningParamWindow ui_; 26 | }; 27 | } 28 | 29 | #endif // BLEND_TOOL_PARAM_WINDOW_H 30 | -------------------------------------------------------------------------------- /godel_plugins/include/godel_plugins/widgets/robot_scan_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_SCAN_CONFIGURATION_H 2 | #define ROBOT_SCAN_CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace godel_plugins 14 | { 15 | /** 16 | * @brief The PoseWidget class 17 | */ 18 | class PoseWidget : public QWidget 19 | { 20 | Q_OBJECT 21 | public: 22 | PoseWidget(QWidget* parent = NULL); 23 | 24 | void set_values(const geometry_msgs::Pose& p); 25 | void set_values(const tf::Transform& t); 26 | tf::Transform get_values(); 27 | 28 | protected: 29 | Ui::PoseWidget ui_; 30 | }; 31 | 32 | /** 33 | * @brief The RobotScanConfigWidget class 34 | */ 35 | class RobotScanConfigWidget : public ParameterWindowBase 36 | { 37 | Q_OBJECT 38 | public: 39 | RobotScanConfigWidget(const godel_msgs::RobotScanParameters& params); 40 | 41 | godel_msgs::RobotScanParameters& params() { return params_; } 42 | 43 | protected: 44 | virtual void update_gui_fields(); 45 | 46 | virtual void update_internal_values(); 47 | 48 | godel_msgs::RobotScanParameters params_; 49 | PoseWidget* world_to_obj_pose_widget_; 50 | PoseWidget* tcp_to_cam_pose_widget_; 51 | Ui::RobotScanConfigWindow ui_; 52 | }; 53 | } 54 | 55 | #endif // ROBOT_SCAN_CONFIGURATION_H 56 | -------------------------------------------------------------------------------- /godel_plugins/include/godel_plugins/widgets/scan_tool_configuration_window.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_TOOL_CONFIGURATION_WINDOW_H 2 | #define SCAN_TOOL_CONFIGURATION_WINDOW_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_plugins 9 | { 10 | 11 | class ScanPlanConfigWidget : public ParameterWindowBase 12 | { 13 | Q_OBJECT 14 | public: 15 | ScanPlanConfigWidget(const godel_msgs::ScanPlanParameters& params); 16 | godel_msgs::ScanPlanParameters& params() { return params_; } 17 | 18 | protected: 19 | virtual void update_gui_fields(); 20 | virtual void update_internal_values(); 21 | virtual int get_quality_combobox_index(); 22 | virtual int get_scan_method_enum_value(); 23 | 24 | godel_msgs::ScanPlanParameters params_; 25 | Ui::ScanToolConfigurationWindow ui_; 26 | }; 27 | } 28 | 29 | #endif // SCAN_TOOL_CONFIGURATION_WINDOW_H 30 | -------------------------------------------------------------------------------- /godel_plugins/include/godel_plugins/widgets/surface_detection_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef SURFACE_DETECTION_CONFIGURATION_H 2 | #define SURFACE_DETECTION_CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_plugins 9 | { 10 | 11 | class SurfaceDetectionConfigWidget : public ParameterWindowBase 12 | { 13 | Q_OBJECT 14 | public: 15 | SurfaceDetectionConfigWidget(const godel_msgs::SurfaceDetectionParameters& params); 16 | 17 | godel_msgs::SurfaceDetectionParameters& params() { return params_; } 18 | 19 | protected: 20 | virtual void update_gui_fields(); 21 | 22 | virtual void update_internal_values(); 23 | 24 | godel_msgs::SurfaceDetectionParameters params_; 25 | Ui::SurfaceDetectionConfigWindow ui_; 26 | }; 27 | } 28 | 29 | #endif // SURFACE_DETECTION_CONFIGURATION_H 30 | -------------------------------------------------------------------------------- /godel_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_plugins 4 | 0.1.0 5 | 6 | A developer's user-interface that interacts with the Godel 7 | blending server. 8 | 9 | 10 | Jonathan Meyer 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | roscpp 16 | rqt_gui 17 | rqt_gui_cpp 18 | rviz 19 | godel_msgs 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /godel_plugins/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | A panel widget that can be used in an rviz instance for surface blending path generation 8 | 9 | 10 | 12 | 13 | A GUI plugin to create and run robot paths for blending surfaces. 14 | 15 | 16 | 17 | 18 | folder 19 | Plugins related to visualization. 20 | 21 | 22 | image-x-generic 23 | A GUI plugin for generating robot paths for surface blending 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /godel_plugins/src/widgets/parameter_window_base.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_plugins/widgets/parameter_window_base.h" 2 | 3 | void godel_plugins::ParameterWindowBase::show() 4 | { 5 | update_gui_fields(); 6 | QMainWindow::show(); 7 | } 8 | 9 | void godel_plugins::ParameterWindowBase::accept_changes_handler() 10 | { 11 | this->update_internal_values(); 12 | Q_EMIT parameters_changed(); 13 | hide(); 14 | } 15 | 16 | void godel_plugins::ParameterWindowBase::cancel_changes_handler() { hide(); } 17 | 18 | void godel_plugins::ParameterWindowBase::save_changes_handler() 19 | { 20 | accept_changes_handler(); 21 | Q_EMIT parameters_save_requested(); 22 | } 23 | -------------------------------------------------------------------------------- /godel_plugins/src/widgets/path_planning_param_window.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_plugins/widgets/path_planning_param_window.h" 2 | 3 | godel_plugins::PathPlanningConfigWidget::PathPlanningConfigWidget( 4 | const godel_msgs::PathPlanningParameters& params) 5 | : params_(params) 6 | { 7 | ui_.setupUi(this); 8 | 9 | connect(ui_.PushButtonAccept, SIGNAL(clicked()), this, SLOT(accept_changes_handler())); 10 | connect(ui_.PushButtonCancel, SIGNAL(clicked()), this, SLOT(cancel_changes_handler())); 11 | connect(ui_.PushButtonSave, SIGNAL(clicked()), this, SLOT(save_changes_handler())); 12 | } 13 | 14 | void godel_plugins::PathPlanningConfigWidget::update_gui_fields() 15 | { 16 | ui_.LineEditDiscretization->setText(QString::number(params_.discretization)); 17 | ui_.LineEditMargin->setText(QString::number(params_.margin)); 18 | ui_.LineEditOverlap->setText(QString::number(params_.overlap)); 19 | ui_.LineEditScanWidth->setText(QString::number(params_.scan_width)); 20 | ui_.LineEditToolRadius->setText(QString::number(params_.tool_radius)); 21 | ui_.LineEditTraverseHeight->setText(QString::number(params_.traverse_height)); 22 | } 23 | 24 | void godel_plugins::PathPlanningConfigWidget::update_internal_values() 25 | { 26 | params_.discretization = ui_.LineEditDiscretization->text().toDouble(); 27 | params_.margin = ui_.LineEditMargin->text().toDouble(); 28 | params_.overlap = ui_.LineEditOverlap->text().toDouble(); 29 | params_.scan_width = ui_.LineEditScanWidth->text().toDouble(); 30 | params_.tool_radius = ui_.LineEditToolRadius->text().toDouble(); 31 | params_.traverse_height = ui_.LineEditTraverseHeight->text().toDouble(); 32 | } 33 | -------------------------------------------------------------------------------- /godel_polygon_offset/README.md: -------------------------------------------------------------------------------- 1 | Godel Polygon Offset 2 | ============== 3 | 4 | This package offsets polygon boundaries for use in Godel blending. 5 | -------------------------------------------------------------------------------- /godel_polygon_offset/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_polygon_offset 4 | 0.0.0 5 | The godel_polygon_offset package implements a method to create offset polygons from an input set of polygon boundaries. This package utilizes the openvoronoi package created by Anders Wallin https://github.com/aewallin/openvoronoi/ 6 | 7 | Dan Solomon 8 | GPLv3 9 | https://github.com/ros-industrial/godel/godel_polygon_offset 10 | Dan Solomon 11 | 12 | catkin 13 | 14 | geometry_msgs 15 | roscpp 16 | godel_process_path_generation 17 | godel_openvoronoi 18 | path_planning_plugins 19 | 20 | message_generation 21 | cmake_modules 22 | 23 | message_runtime 24 | 25 | 26 | -------------------------------------------------------------------------------- /godel_polygon_offset/src/offset_sorter.patch: -------------------------------------------------------------------------------- 1 | @@ -330,6 +330,12 @@ 2 | return py_offsets; 3 | } 4 | 5 | + /// Return reference to machining graph 6 | + const MachiningGraph& getMachiningGraph() const { 7 | + return g; 8 | + } 9 | + 10 | + 11 | protected: 12 | /// set of Loops, sorted by decreasing offset-distance 13 | std::multiset distance_sorted_loops; 14 | -------------------------------------------------------------------------------- /godel_polygon_offset/srv/OffsetPolygon.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Polygon[] polygons # Boundary order CCW external, CW internal 2 | float64 offset_distance # Distance (m) to perform typical offset 3 | float64 initial_offset # If specified (>0), distance (m) to perform initial offset 4 | float64 discretization # Max Distance (m) used to discretize lines/arcs 5 | --- 6 | geometry_msgs/Polygon[] offset_polygons # Ordered list of offset polygons 7 | float64[] offsets # List of distances, each offset corresponds to offset_polygon depth -------------------------------------------------------------------------------- /godel_process_execution/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_process_execution) 3 | add_definitions(-std=c++11) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | abb_file_suite 7 | godel_msgs 8 | godel_utils 9 | industrial_robot_simulator_service 10 | trajectory_msgs 11 | keyence_experimental 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS 16 | include 17 | CATKIN_DEPENDS 18 | abb_file_suite 19 | godel_msgs 20 | godel_utils 21 | industrial_robot_simulator_service 22 | trajectory_msgs 23 | keyence_experimental 24 | ) 25 | 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | 31 | add_executable(abb_blend_process_service_node 32 | src/abb_blend_process_service_node.cpp 33 | src/abb_blend_process_service.cpp 34 | src/process_utils.cpp 35 | ) 36 | 37 | add_executable(blend_process_service_node 38 | src/blend_process_service_node.cpp 39 | src/blend_process_service.cpp 40 | src/process_utils.cpp 41 | ) 42 | 43 | add_executable(keyence_process_service_node 44 | src/keyence_process_service_node.cpp 45 | src/keyence_process_service.cpp 46 | src/process_utils.cpp 47 | ) 48 | 49 | add_dependencies(abb_blend_process_service_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 50 | 51 | add_dependencies(blend_process_service_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 52 | 53 | add_dependencies(keyence_process_service_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 54 | 55 | 56 | target_link_libraries(abb_blend_process_service_node 57 | ${catkin_LIBRARIES} 58 | ) 59 | 60 | target_link_libraries(blend_process_service_node 61 | ${catkin_LIBRARIES} 62 | ) 63 | 64 | target_link_libraries(keyence_process_service_node 65 | ${catkin_LIBRARIES} 66 | ) 67 | -------------------------------------------------------------------------------- /godel_process_execution/include/godel_process_execution/abb_blend_process_service.h: -------------------------------------------------------------------------------- 1 | #ifndef ABB_BLEND_PROCESS_SERVICE_H 2 | #define ABB_BLEND_PROCESS_SERVICE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_process_execution 9 | { 10 | 11 | class AbbBlendProcessService 12 | { 13 | public: 14 | AbbBlendProcessService(ros::NodeHandle& nh); 15 | 16 | /** 17 | * Currently forwards the godel_msgs::TrajectoryExecution on to the corresponding 18 | * MoveIt node. The idea though is that abstracting 'execution' will give us more flexibility 19 | * later to implement our own process parameters related to execution. 20 | */ 21 | void executionCallback(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 22 | bool executeProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 23 | bool simulateProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 24 | 25 | private: 26 | ros::NodeHandle nh_; 27 | ros::ServiceClient real_client_; 28 | ros::ServiceClient sim_client_; 29 | actionlib::SimpleActionServer process_exe_action_server_; 30 | bool j23_coupled_; 31 | }; 32 | } 33 | 34 | #endif // ABB_BLEND_PROCESS_SERVICE_H 35 | -------------------------------------------------------------------------------- /godel_process_execution/include/godel_process_execution/blend_process_service.h: -------------------------------------------------------------------------------- 1 | #ifndef BLEND_PROCESS_SERVICE_H 2 | #define BLEND_PROCESS_SERVICE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_process_execution 9 | { 10 | 11 | class BlendProcessService 12 | { 13 | public: 14 | BlendProcessService(ros::NodeHandle& nh); 15 | 16 | void executionCallback(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 17 | bool executeProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 18 | bool simulateProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 19 | 20 | private: 21 | ros::NodeHandle nh_; 22 | ros::ServiceClient real_client_; 23 | ros::ServiceClient sim_client_; 24 | actionlib::SimpleActionServer process_exe_action_server_; 25 | bool j23_coupled_; 26 | }; 27 | } 28 | 29 | #endif // BLEND_PROCESS_SERVICE_H 30 | -------------------------------------------------------------------------------- /godel_process_execution/include/godel_process_execution/keyence_process_service.h: -------------------------------------------------------------------------------- 1 | #ifndef KEYENCE_PROCESS_SERVICE_H 2 | #define KEYENCE_PROCESS_SERVICE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_process_execution 9 | { 10 | 11 | class KeyenceProcessService 12 | { 13 | public: 14 | KeyenceProcessService(ros::NodeHandle& nh); 15 | 16 | /** 17 | * Currently forwards the godel_msgs::TrajectoryExecution on to the corresponding 18 | * MoveIt node. The idea though is that abstracting 'execution' will give us more flexibility 19 | * later to implement our own process parameters related to execution. 20 | */ 21 | void executionCallback(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 22 | bool executeProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 23 | bool simulateProcess(const godel_msgs::ProcessExecutionGoalConstPtr &goal); 24 | 25 | private: 26 | ros::NodeHandle nh_; 27 | ros::ServiceClient real_client_; 28 | ros::ServiceClient sim_client_; 29 | actionlib::SimpleActionServer process_exe_action_server_; 30 | ros::ServiceClient keyence_client_; 31 | ros::ServiceClient reset_scan_server_; 32 | }; 33 | } 34 | 35 | #endif // KEYENCE_PROCESS_SERVICE_H 36 | -------------------------------------------------------------------------------- /godel_process_execution/launch/abb_blend_process_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /godel_process_execution/launch/blend_process_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /godel_process_execution/launch/keyence_process_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /godel_process_execution/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_process_execution 4 | 0.1.0 5 | 6 | This package is responsible for the execution of process plans generated 7 | by packages like godel_process_planning. It handles motion and IO for the 8 | tool, and is specialized for different robot vendors. 9 | 10 | 11 | Jonathan Meyer 12 | Apache 2.0 13 | 14 | catkin 15 | 16 | abb_file_suite 17 | godel_msgs 18 | godel_utils 19 | industrial_robot_simulator_service 20 | trajectory_msgs 21 | keyence_experimental 22 | swri_profiler 23 | 24 | 25 | -------------------------------------------------------------------------------- /godel_process_execution/src/abb_blend_process_service_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "abb_blend_process_service_node"); 8 | 9 | ros::NodeHandle nh; 10 | 11 | godel_process_execution::AbbBlendProcessService process_executor(nh); 12 | 13 | ros::spin(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /godel_process_execution/src/blend_process_service_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "blend_process_service_node"); 8 | 9 | ros::NodeHandle nh; 10 | 11 | godel_process_execution::BlendProcessService process_executor(nh); 12 | 13 | ros::spin(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /godel_process_execution/src/keyence_process_service_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "keyence_process_service_node"); 8 | ros::NodeHandle nh; 9 | 10 | godel_process_execution::KeyenceProcessService process_executor(nh); 11 | 12 | ros::spin(); 13 | return 0; 14 | } 15 | -------------------------------------------------------------------------------- /godel_process_execution/src/process_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "process_utils.h" 2 | 3 | void godel_process_execution::appendTrajectory(trajectory_msgs::JointTrajectory& original, 4 | const trajectory_msgs::JointTrajectory& next) 5 | { 6 | ros::Duration last_t = 7 | original.points.empty() ? ros::Duration(0.0) : original.points.back().time_from_start; 8 | for (std::size_t i = 0; i < next.points.size(); ++i) 9 | { 10 | trajectory_msgs::JointTrajectoryPoint pt = next.points[i]; 11 | pt.time_from_start += last_t; 12 | 13 | original.points.push_back(pt); 14 | } 15 | } -------------------------------------------------------------------------------- /godel_process_execution/src/process_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_GODEL_PROCESS_UTILS_H 2 | #define PATH_GODEL_PROCESS_UTILS_H 3 | 4 | #include 5 | 6 | namespace godel_process_execution 7 | { 8 | 9 | void appendTrajectory(trajectory_msgs::JointTrajectory& original, 10 | const trajectory_msgs::JointTrajectory& next); 11 | } 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /godel_process_path_generation/README.md: -------------------------------------------------------------------------------- 1 | Godel Process Path Generation 2 | ============== 3 | 4 | This package creates the high-level process path for surface blending. 5 | 6 | # Running Visualization # 7 | 8 | - Note: If package godel_polygon_offset is used, start its node: 9 | - rosrun godel_polygon_offset godel_polygon_offset_node 10 | - rosrun rviz rviz (use path_visualization.rviz in test) 11 | - rosrun godel_process_path_generation process_visualization_node --demo 12 | 13 | # Input # 14 | 15 | - Pointcloud of a surface. 16 | - Outside edge of surface is assumed to be boundary. 17 | - Internal edges are currently ignored, but will be incorporated as features in the future. 18 | - Tool radius or diameter 19 | - Desired offset for each loop (absolute) 20 | - Safe height for rapid transit moves 21 | 22 | # Output # 23 | 24 | - ProcessPlan for the entire surface. 25 | - ProcessPlan should be sent to TrajectoryPlanner for trajectory planning. 26 | 27 | 28 | # Classes # 29 | **MeshImporter** 30 | 31 | - Input: pcl::Mesh 32 | - Actions 33 | - fitMeshToPlane 34 | - Plane represented by origin at COM of mesh, and plane normal 35 | - flattenToPlane(vector\) 36 | - transformTo2D(vector\) 37 | - fIndPointOnExternalBoundary(const &pcl::mesh) 38 | - extractExternalBoundary(const &pcl::mesh, etc) 39 | - Member Variables 40 | - pcl::Mesh pointcloud 41 | - transform (calculated from FitMeshToPlane) 42 | - transform from global coordinates to local plane coordinates 43 | - vector\ boundary 44 | 45 | -------------------------------------------------------------------------------- /godel_process_path_generation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_process_path_generation 4 | 0.0.0 5 | 6 | The godel_process_path_generation package creates the high-level process path for surface blending. 7 | 8 | 9 | Daniel Solomon 10 | Daniel Solomon 11 | Apache2 12 | https://github.com/ros-industrial-consortium/godel 13 | 14 | catkin 15 | 16 | roscpp 17 | godel_msgs 18 | tf 19 | geometry_msgs 20 | visualization_msgs 21 | pcl_ros 22 | eigen_conversions 23 | moveit_core 24 | moveit_ros_planning_interface 25 | 26 | trajectory_msgs 27 | cmake_modules 28 | 29 | 30 | -------------------------------------------------------------------------------- /godel_process_path_generation/src/process_pt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * process_pt.cpp 20 | * 21 | * Created on: May 12, 2014 22 | * Author: Dan Solomon 23 | */ 24 | 25 | #include "godel_process_path_generation/process_pt.h" 26 | 27 | namespace descartes 28 | { 29 | 30 | } /* namespace descartes */ 31 | -------------------------------------------------------------------------------- /godel_process_path_generation/src/process_transition.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Apache License) 3 | * 4 | * Copyright (c) 2014, Southwest Research Institute 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | /* 19 | * process_transition.cpp 20 | * 21 | * Created on: May 14, 2014 22 | * Author: Dan Solomon 23 | */ 24 | 25 | #include "godel_process_path_generation/process_transition.h" 26 | 27 | namespace descartes 28 | { 29 | 30 | bool JointVelocityConstraint::isValid() 31 | { 32 | if (min.size() != max.size() || min.size() != desired.size()) 33 | { 34 | return false; 35 | } 36 | for (size_t ii = 0; ii < min.size(); ++ii) 37 | { 38 | if (min.at(ii) > max.at(ii) || min.at(ii) > desired.at(ii) || desired.at(ii) > max.at(ii)) 39 | { 40 | return false; 41 | } 42 | } 43 | return true; 44 | } 45 | 46 | bool LinearVelocityConstraint::isValid() 47 | { 48 | if (min > max || min > desired || desired > max) 49 | { 50 | return false; 51 | } 52 | return true; 53 | } 54 | 55 | bool RotationalVelocityConstraint::isValid() 56 | { 57 | if (min > max || min > desired || desired > max) 58 | { 59 | return false; 60 | } 61 | return true; 62 | } 63 | 64 | } /* namespace descartes */ 65 | -------------------------------------------------------------------------------- /godel_process_path_generation/srv/VisualizeBlendingPlan.srv: -------------------------------------------------------------------------------- 1 | # Service used to visualize cartesian blending plan of single surface. 2 | 3 | godel_msgs/BlendingPlanParameters params 4 | godel_msgs/SurfaceBoundaries surface 5 | --- 6 | visualization_msgs/Marker path 7 | duration[] sleep_times 8 | -------------------------------------------------------------------------------- /godel_process_planning/config/process_planning.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | process_planning_params: 3 | blend_params: 4 | spindle_speed: 0.0 5 | approach_speed: 0.005 6 | blending_speed: 0.3 7 | retract_speed: 0.02 8 | traverse_speed: 0.05 9 | z_adjust: 0.01 10 | scan_params: 11 | approach_distance: 0.15 12 | traverse_speed: 0.05 13 | quality_metric: 0 14 | window_width: 0.02 15 | min_qa_value: 0.05 16 | max_qa_value: 0.05 17 | ... 18 | -------------------------------------------------------------------------------- /godel_process_planning/launch/process_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_process_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_process_planning 4 | 0.1.0 5 | 6 | The process planning package handles joint trajectory planning for (at the 7 | moment) two distinct processes: blending and laser line scanning. This package replaces the earlier 'godel_path_planning'. 8 | 9 | 10 | Jonathan Meyer 11 | Jonathan Meyer 12 | Apache 2.0 13 | 14 | catkin 15 | 16 | descartes_core 17 | descartes_moveit 18 | descartes_planner 19 | descartes_trajectory 20 | godel_msgs 21 | moveit_ros_planning_interface 22 | roscpp 23 | 24 | 25 | -------------------------------------------------------------------------------- /godel_process_planning/src/generate_motion_plan.h: -------------------------------------------------------------------------------- 1 | #ifndef GODEL_GENERATE_MOTION_PLAN_H 2 | #define GODEL_GENERATE_MOTION_PLAN_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace godel_process_planning 9 | { 10 | 11 | /** 12 | * @brief This is a helper function for doing the joint level trajectory planning for a 13 | * a robot from a given \e start_state to and through the process path defined by \e 14 | * traj. 15 | * @param model A descartes robot model for this process/tool 16 | * @param traj A sequence of descartes points encapsulating the path tolerances 17 | * @param moveit_model A moveit robot model corresponding to the robot used 18 | * @param move_group_name The name of the move group being manipulated 19 | * @param start_state The initial position of the robot 20 | * @param plan Output parameter - the approach, process, and departure joint paths. 21 | * NOTE THAT ProcessPlan::type is NOT set. 22 | * @return True on planning success, false otherwise 23 | */ 24 | bool generateMotionPlan(const descartes_core::RobotModelPtr model, 25 | const std::vector& traj, 26 | moveit::core::RobotModelConstPtr moveit_model, 27 | const std::string& move_group_name, 28 | const std::vector& start_state, 29 | godel_msgs::ProcessPlan& plan); 30 | 31 | 32 | } 33 | 34 | #endif // GODEL_GENERATE_MOTION_PLAN_H 35 | -------------------------------------------------------------------------------- /godel_process_planning/src/trajectory_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORY_UTILS_H 2 | #define TRAJECTORY_UTILS_H 3 | 4 | #include 5 | 6 | namespace godel_process_planning 7 | { 8 | // Cartesian Interpolation 9 | typedef std::vector > PoseVector; 10 | 11 | /** 12 | * @brief Creates a vector of poses representing linear spatial and rotational interpolation 13 | * between starting and ending poses. 14 | * @param start Beginning pose 15 | * @param stop Terminating pose 16 | * @param ds The cartesian distance (m) between intermediate points 17 | * @return Sequence of poses 18 | */ 19 | PoseVector interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, 20 | double ds); 21 | 22 | // Joint Interpolation 23 | typedef std::vector > JointVector; 24 | 25 | /** 26 | * @brief Creates a vector of joint poses linearly interpolating from start to stop 27 | * @param start Initial joint configuration 28 | * @param stop Final joint configuration 29 | * @param dtheta Maximum joint step (radians) between intermediate points 30 | * @return 31 | */ 32 | JointVector interpolateJoint(const std::vector& start, const std::vector& stop, 33 | double dtheta); 34 | } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/README.md: -------------------------------------------------------------------------------- 1 | # abb_file_suite 2 | A suite of tools used for interfacing ROS-I and an ABB robot through the use of module files that are generated and downloaded on the fly. 3 | 4 | ## ABB Controller Side 5 | The `abb_motion_ftp_downloader` interface uses the standard ROS-I State Server and a custom motion server. Files for it can be found under the `rapid/` subdirectory. The system works by listening for the existence of a specific file in the `HOME/PARTMODULES` directory on the controller. Files are uploaded via FTP using [libcurl](http://curl.haxx.se/libcurl/). This library can be downloaded through the apt-get interface: 6 | 7 | ``` 8 | sudo apt-get install libcurl4-openssl-dev 9 | ``` 10 | 11 | ## Rapid Generator 12 | When using the Rapid generation routines, be sure to flush/close your output file before sending it to the 'execute program' service. 13 | 14 | -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | abb_file_suite 4 | 0.1.0 5 | 6 | An alternative ROS driver for the ABB series of robots that creates RAPID programs 7 | in code and then FTPs the results to the robot for execution. 8 | 9 | 10 | Jonathan Meyer 11 | 12 | Jonathan Meyer 13 | Zach Bennett 14 | Apache 2.0 15 | 16 | catkin 17 | 18 | roscpp 19 | trajectory_msgs 20 | message_runtime 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/rapid/mGodel_Main.mod: -------------------------------------------------------------------------------- 1 | MODULE mGodel_DemoMain 2 | PROC Godel_Main() 3 | !Delete Files if they exist 4 | IF IsFile("HOME:/PARTMODULES/mGodelBlend.mod") RemoveFile "HOME:/PARTMODULES/mGodelBlend.mod"; 5 | IF ModExist("mGodel_Blend") EraseModule("mGodel_Blend"); 6 | 7 | WHILE true DO 8 | !Wait for Blend File 9 | WaitUntil IsFile("HOME:/PARTMODULES/mGodelBlend.mod"); 10 | WaitTime 0.25; 11 | Load "HOME:/PartModules" \File:="mGodelBlend.MOD"; 12 | %"Godel_Blend"%; 13 | UnLoad "HOME:/PartModules" \File:="mGodelBlend.MOD"; 14 | RemoveFile "HOME:/PARTMODULES/mGodelBlend.mod"; 15 | ENDWHILE 16 | ! 17 | ENDPROC 18 | 19 | ENDMODULE -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/rapid_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rapid_generator) 3 | 4 | include_directories(include) 5 | add_library(rapid_generator src/rapid_emitter.cpp) -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_data_structures.h: -------------------------------------------------------------------------------- 1 | #ifndef RAPID_DATA_STRUCTURES_H 2 | #define RAPID_DATA_STRUCTURES_H 3 | 4 | #include 5 | #include 6 | 7 | namespace rapid_emitter 8 | { 9 | 10 | /** 11 | * @brief A joint trajectory point that can be converted to a RAPID 12 | * joint target. 13 | */ 14 | struct TrajectoryPt 15 | { 16 | typedef std::vector value_type; 17 | 18 | TrajectoryPt(const std::vector& positions, double duration = 0.0) 19 | : positions_(positions), duration_(duration) 20 | { 21 | } 22 | 23 | value_type positions_; // degrees 24 | double duration_; // seconds 25 | }; 26 | 27 | /** 28 | * @brief This structure is used in combination with a vector of trajectory points to 29 | * generate any entire rapid program. Some fields aren't used by the default 30 | * software, but interact with proprietary extensions like Wolf Robotic's 31 | * Wolfware. 32 | */ 33 | struct ProcessParams 34 | { 35 | double spindle_speed; // Tool rotation speed 36 | double tcp_speed; // Tool linear traversal speed 37 | double force; // For non-wolfware software, a general identification of force 38 | std::string output_name; // The I/O that toggles the tool power; We assume 0 is off and 1 is on. 39 | bool wolf_mode; // In the case of Wolfware software, we emit special instructions. 40 | double slide_force; // For WolfWare, a meaure of the cross-slide force (?) 41 | }; 42 | } 43 | 44 | #endif // RAPID_DATA_STRUCTURES_H 45 | -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader_node.cpp: -------------------------------------------------------------------------------- 1 | #include "abb_file_suite/abb_motion_ftp_downloader.h" 2 | #include 3 | 4 | // This topic will work with ROS-I standard interfaces such as the action server 5 | // by default 6 | const static std::string DEFAULT_JOINT_LISTENING_TOPIC = "joint_path_command"; 7 | 8 | int main(int argc, char** argv) 9 | { 10 | ros::init(argc, argv, "abb_motion_ftp_donwloader"); 11 | 12 | ros::NodeHandle nh, pnh("~"); 13 | 14 | // load parameters 15 | std::string ip, topic; 16 | bool j23_coupled; 17 | pnh.param("controller_ip", ip, ""); 18 | pnh.param("topic", topic, DEFAULT_JOINT_LISTENING_TOPIC); 19 | 20 | std::string username, password; 21 | pnh.param("ftp_user", username, ""); 22 | pnh.param("ftp_pwd", password, ""); 23 | 24 | // Note this lookup is done using a non-private node handle. We assume we are looking up 25 | // this parameter set in the launch file for many abb drivers. 26 | nh.param("J23_coupled", j23_coupled, false); 27 | 28 | abb_file_suite::AbbMotionFtpDownloader ftp(ip, topic, nh, username, password, j23_coupled); 29 | 30 | ROS_INFO("ABB FTP-Trajectory-Downloader (ip %s) initialized. Listening on topic %s.", ip.c_str(), 31 | topic.c_str()); 32 | ros::spin(); 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/src/ftp_upload.h: -------------------------------------------------------------------------------- 1 | #ifndef FTP_UPLOAD_H 2 | #define FTP_UPLOAD_H 3 | 4 | #include 5 | 6 | namespace abb_file_suite 7 | { 8 | 9 | bool uploadFile(const std::string& ftp_addr, const std::string& filepath, 10 | const std::string& user_name, const std::string& password); 11 | } 12 | 13 | #endif // FTP_UPLOAD_H 14 | -------------------------------------------------------------------------------- /godel_robots/abb/abb_file_suite/srv/ExecuteProgram.srv: -------------------------------------------------------------------------------- 1 | # Absolute file path to the RAPID file that will be uploaded to the Robot 2 | string file_path 3 | 4 | --- 5 | # EMPTY - future improvements might inform of failure to establish connection -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_5_90_ikfast_manipulator_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(abb_irb1200_5_90_ikfast_manipulator_plugin) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | moveit_core 8 | pluginlib 9 | roscpp 10 | tf_conversions 11 | ) 12 | 13 | include_directories(${catkin_INCLUDE_DIRS}) 14 | 15 | catkin_package( 16 | LIBRARIES 17 | CATKIN_DEPENDS 18 | moveit_core 19 | pluginlib 20 | roscpp 21 | tf_conversions 22 | ) 23 | 24 | include_directories(include) 25 | 26 | set(IKFAST_LIBRARY_NAME abb_irb1200_5_90_manipulator_moveit_ikfast_plugin) 27 | 28 | find_package(LAPACK REQUIRED) 29 | 30 | add_library(${IKFAST_LIBRARY_NAME} src/abb_irb1200_5_90_manipulator_ikfast_moveit_plugin.cpp) 31 | target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) 32 | 33 | install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 34 | 35 | install( 36 | FILES 37 | abb_irb1200_5_90_manipulator_moveit_ikfast_plugin_description.xml 38 | DESTINATION 39 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 40 | ) 41 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_5_90_ikfast_manipulator_plugin/abb_irb1200_5_90_manipulator_moveit_ikfast_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | IKFast61 plugin for closed-form kinematics 5 | 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_5_90_ikfast_manipulator_plugin/abb_irb1200_manipulator_moveit_ikfast_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | IKFast61 plugin for closed-form kinematics 5 | 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_5_90_ikfast_manipulator_plugin/update_ikfast_plugin.sh: -------------------------------------------------------------------------------- 1 | rosrun moveit_kinematics create_ikfast_moveit_plugin.py abb_irb1200_5_90 manipulator abb_irb1200_5_90_ikfast_manipulator_plugin /home/jmeyer/ros/abb_ws/src/abb_irb1200_5_90_ikfast_manipulator_plugin/src/abb_irb1200_5_90_manipulator_ikfast_solver.cpp -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(abb_irb1200_support) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | install(DIRECTORY config launch meshes urdf 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/config/joint_names_irb1200_5_90.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] 2 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/launch/load_irb1200_5_90.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/launch/robot_interface_download_irb1200_5_90.launch: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/launch/robot_state_visualize_irb1200_5_90.launch: -------------------------------------------------------------------------------- 1 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/launch/test_irb1200_5_90.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/base_link.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_1.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_2.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_3.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_4.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_5.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/abb_irb1200_support/meshes/irb1200_5_90/collision/link_6.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/abb_irb1200_support/urdf/irb1200_5_90.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: godel_irb1200_support 4 | relative_path: urdf/irb1200_workspace.xacro 5 | SRDF: 6 | relative_path: config/irb1200_workspace.srdf 7 | CONFIG: 8 | author_name: Jonathan Meyer 9 | author_email: jmeyer@swri.org 10 | generated_timestamp: 1489451576 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_irb1200_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: "" 3 | action_ns: joint_trajectory_action 4 | type: FollowJointTrajectory 5 | joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_controller 3 | joints: 4 | - joint_1 5 | - joint_2 6 | - joint_3 7 | - joint_4 8 | - joint_5 9 | - joint_6 10 | - name: fake_manipulator_asus_controller 11 | joints: 12 | - joint_1 13 | - joint_2 14 | - joint_3 15 | - joint_4 16 | - joint_5 17 | - joint_6 18 | - name: fake_manipulator_keyence_controller 19 | joints: 20 | - joint_1 21 | - joint_2 22 | - joint_3 23 | - joint_4 24 | - joint_5 25 | - joint_6 26 | - name: fake_manipulator_tcp_controller 27 | joints: 28 | - joint_1 29 | - joint_2 30 | - joint_3 31 | - joint_4 32 | - joint_5 33 | - joint_6 34 | - name: fake_manipulator_ensenso_controller 35 | joints: 36 | - joint_1 37 | - joint_2 38 | - joint_3 39 | - joint_4 40 | - joint_5 41 | - joint_6 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | joint_1: 6 | has_velocity_limits: true 7 | max_velocity: 5.027 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | joint_2: 11 | has_velocity_limits: true 12 | max_velocity: 4.189 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | joint_3: 16 | has_velocity_limits: true 17 | max_velocity: 5.236 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | joint_4: 21 | has_velocity_limits: true 22 | max_velocity: 6.981 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | joint_5: 26 | has_velocity_limits: true 27 | max_velocity: 7.069 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | joint_6: 31 | has_velocity_limits: true 32 | max_velocity: 10.472 33 | has_acceleration_limits: false 34 | max_acceleration: 0 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: abb_irb1200_5_90_manipulator_kinematics/IKFastKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 6 | manipulator_asus: 7 | kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin 8 | kinematics_solver_search_resolution: 0.005 9 | kinematics_solver_timeout: 0.005 10 | kinematics_solver_attempts: 3 11 | manipulator_keyence: 12 | kinematics_solver: abb_irb1200_5_90_manipulator_kinematics/IKFastKinematicsPlugin 13 | kinematics_solver_search_resolution: 0.005 14 | kinematics_solver_timeout: 0.005 15 | kinematics_solver_attempts: 3 16 | manipulator_tcp: 17 | kinematics_solver: abb_irb1200_5_90_manipulator_kinematics/IKFastKinematicsPlugin 18 | kinematics_solver_search_resolution: 0.005 19 | kinematics_solver_timeout: 0.005 20 | kinematics_solver_attempts: 3 21 | manipulator_ensenso: 22 | kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin 23 | kinematics_solver_search_resolution: 0.005 24 | kinematics_solver_timeout: 0.005 25 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/irb1200_robot_ftp_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/irb1200_workspace_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/irb1200_workspace_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_irb1200_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the irb1200_workspace with the MoveIt! Motion Planning Framework 7 | 8 | Jonathan Meyer 9 | Jonathan Meyer 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit/issues 15 | https://github.com/ros-planning/moveit 16 | 17 | catkin 18 | 19 | moveit_ros_move_group 20 | moveit_kinematics 21 | moveit_planners_ompl 22 | moveit_ros_visualization 23 | joint_state_publisher 24 | robot_state_publisher 25 | xacro 26 | 27 | 28 | godel_irb1200_support 29 | godel_irb1200_support 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_irb1200_support) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY 9 | config 10 | launch 11 | rviz 12 | urdf 13 | yaml 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 15 | ) 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/config/blending_plan.yaml: -------------------------------------------------------------------------------- 1 | blending_plan: 2 | tool_radius: 0.0125 # (m) actual radius of blending tool 3 | margin: 0.001 # (m) additional distance to leave near surface boundaries 4 | overlap: 0.00 # (m) overlap distance between adjacent paths 5 | approach_spd: 0.005 # (m/s) Speed to approach surface of part 6 | blending_spd: 0.3 # (m/s) Speed to perform blending 7 | retract_spd: 0.02 # (m/s) Speed to move away from surface of part 8 | traverse_spd: 0.3 # (m/s) Speed to travel while at traverse height 9 | discretization: 0.0025 # (m) How densely to space adjacent Cartesian points 10 | safe_traverse_height: 0.05 # (m) height above surface to rise during rapid traversal moves 11 | min_boundary_length: .03 # (m) Boundaries below threshold are ignored during process path creation 12 | tool_force: 0.0 # (kg) 13 | spindle_speed: 0.0 # (rad/s) -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/config/plugins.yaml: -------------------------------------------------------------------------------- 1 | meshing_plugin_name: "concave_hull_mesher::ConcaveHullMesher" 2 | blend_tool_planning_plugin_name: "path_planning_plugins::openveronoi::BlendPlanner" 3 | scan_tool_planning_plugin_name: "path_planning_plugins::openveronoi::ScanPlanner" 4 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/config/point_cloud_descriptions.yaml: -------------------------------------------------------------------------------- 1 | frame_id: world_frame 2 | point_cloud_descriptions: 3 | - x: 0.5 4 | y: 0.2 5 | z: 0.36 6 | rx: 0.0 7 | ry: 0.0 8 | rz: 0.0 9 | l: 0.1 10 | w: 0.19 11 | h: 0.0 12 | resolution: 0.002 13 | - x: 0.6 14 | y: -0.1 15 | z: 0.1 16 | rx: 0.0 17 | ry: 0.0 18 | rz: 0.0 19 | l: 0.2 20 | w: 0.1 21 | h: 0.0 22 | resolution: 0.002 23 | - x: 0.2 24 | y: -0.5 25 | z: 0.3 26 | rx: 0.0 27 | ry: 0.0 28 | rz: 0.0 29 | l: 0.2 30 | w: 0.2 31 | h: 0.0 32 | resolution: 0.002 33 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/config/robot_scan.yaml: -------------------------------------------------------------------------------- 1 | robot_scan: 2 | group_name: manipulator_asus 3 | home_position: home_asus 4 | world_frame: world_frame 5 | tcp_frame: kinect2_move_frame 6 | tcp_to_cam_pose: 7 | trans: 8 | x: 0 9 | y: 0 10 | z: 0 11 | quat: 12 | x: 0 13 | y: 0 14 | z: 0 15 | w: 1 16 | world_to_obj_pose: 17 | trans: 18 | x: 0.5 19 | y: 0.0 20 | z: 0.5 21 | quat: 22 | x: 0 23 | y: 0 24 | z: 0 25 | w: 1 26 | cam_to_obj_zoffset: 0.6 27 | cam_to_obj_xoffset: 0.5 28 | cam_tilt_angle: 1.57 29 | sweep_angle_start: 0 30 | sweep_angle_end: 0.25 31 | scan_topic: sensor_point_cloud 32 | scan_target_frame: world_frame 33 | reachable_scan_points_ratio: 1.0 34 | stop_on_planning_error: true 35 | num_scan_points: 3 36 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/config/scan_plan.yaml: -------------------------------------------------------------------------------- 1 | scan_plan: 2 | scan_width: 0.01 3 | traverse_speed: 0.2 4 | approach_distance: 0.15 5 | overlap: 0.001 6 | margin: 0 7 | quality_metric: "rms" 8 | window_width: 0.02 9 | min_qa_value: 0.05 10 | max_qa_value: 0.05 11 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/config/surface_detection.yaml: -------------------------------------------------------------------------------- 1 | surface_detection: 2 | frame_id: "world_frame" 3 | k_search: 20 4 | marker_alpha: 1 5 | 6 | meanK: 10 7 | stdv_threshold: 4 8 | 9 | rg_min_cluster_size: 100 10 | rg_max_cluster_size: 10000 11 | rg_neighbors: 20 12 | rg_smoothness_threshold: 0.07853981633974483 13 | rg_curvature_threshold: 2.0 14 | 15 | stout_mean: 1.0 16 | stout_stdev_threshold: 3.0 17 | 18 | voxel_leaf: 0.005 19 | tabletop_seg_distance_thresh: 0.005 20 | use_tabletop_segmentation: False 21 | ignore_largest_cluster: False 22 | 23 | mls_point_density: 100 24 | mls_upsampling_radius: 0.01 25 | mls_search_radius: 0.04 26 | 27 | pa_enabled: True 28 | pa_seg_max_iterations: 200 29 | pa_seg_dist_threshold: 0.01 30 | pa_sac_plane_distance: 0.01 31 | pa_kdtree_radius: 0.01 32 | 33 | use_octomap: False 34 | occupancy_threshold: 0.5 35 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/meshes/visual/automate_demo_cell_simplified.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/abb/godel_irb1200/godel_irb1200_support/meshes/visual/automate_demo_cell_simplified.stl -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_irb1200_support 4 | 0.1.0 5 | 6 | Support package for a Godel system using ABB1200. Includes blending and calibration files. 7 | 8 | 9 | Jonathan Meyer 10 | 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | godel_surface_detection 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb1200/godel_irb1200_support/urdf/irb1200_workspace.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/abb_irb2400_descartes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(abb_irb2400_descartes) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | descartes_core 8 | descartes_moveit 9 | irb2400_ikfast_manipulator_plugin 10 | pluginlib 11 | ) 12 | 13 | find_package(rosconsole_bridge REQUIRED) 14 | find_package(Boost REQUIRED) 15 | find_package(Eigen REQUIRED) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS 19 | include 20 | LIBRARIES 21 | ${PROJECT_NAME} 22 | CATKIN_DEPENDS 23 | descartes_core 24 | descartes_moveit 25 | irb2400_ikfast_manipulator_plugin 26 | pluginlib 27 | ) 28 | 29 | include_directories(include 30 | ${catkin_INCLUDE_DIRS} 31 | ${Boost_INCLUDE_DIRS} 32 | ${Eigen_INCLUDE_DIRS} 33 | ) 34 | 35 | 36 | add_library(${PROJECT_NAME} 37 | src/abb_irb2400_robot_model.cpp 38 | ) 39 | 40 | target_link_libraries(${PROJECT_NAME} 41 | ${catkin_LIBRARIES} 42 | ) 43 | 44 | install(TARGETS ${PROJECT_NAME} 45 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | 50 | # Mark cpp header files for installation 51 | install(DIRECTORY include/${PROJECT_NAME}/ 52 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 53 | FILES_MATCHING PATTERN "*.h" 54 | PATTERN ".svn" EXCLUDE 55 | ) 56 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/abb_irb2400_descartes/abb_irb2400_descartes_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is a robot model adapter for of the abb irb2400 for descartes robot model. 5 | 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/abb_irb2400_descartes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | abb_irb2400_descartes 4 | 0.1.0 5 | 6 | Defines a Descartes RobotModel plugin using the irb2400_ikfast_manipulator_plugin 7 | for kinematics. 8 | 9 | 10 | Jonathan Meyer 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | descartes_core 16 | descartes_moveit 17 | irb2400_ikfast_manipulator_plugin 18 | pluginlib 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: godel_irb2400_support 4 | relative_path: urdf/irb2400_workspace.xacro 5 | SRDF: 6 | relative_path: config/irb2400_workspace.srdf 7 | CONFIG: 8 | author_name: AustinDeric 9 | author_email: austin.deric@gmail.com 10 | generated_timestamp: 1490130796 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_irb2400_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: "" 3 | action_ns: joint_trajectory_action 4 | type: FollowJointTrajectory 5 | joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_controller 3 | joints: 4 | - joint_1 5 | - joint_2 6 | - joint_3 7 | - joint_4 8 | - joint_5 9 | - joint_6 10 | - name: fake_manipulator_asus_controller 11 | joints: 12 | - joint_1 13 | - joint_2 14 | - joint_3 15 | - joint_4 16 | - joint_5 17 | - joint_6 18 | - name: fake_manipulator_keyence_controller 19 | joints: 20 | - joint_1 21 | - joint_2 22 | - joint_3 23 | - joint_4 24 | - joint_5 25 | - joint_6 26 | - name: fake_manipulator_tcp_controller 27 | joints: 28 | - joint_1 29 | - joint_2 30 | - joint_3 31 | - joint_4 32 | - joint_5 33 | - joint_6 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | joint_1: 6 | has_velocity_limits: true 7 | max_velocity: 2.618 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | joint_2: 11 | has_velocity_limits: true 12 | max_velocity: 2.618 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | joint_3: 16 | has_velocity_limits: true 17 | max_velocity: 2.618 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | joint_4: 21 | has_velocity_limits: true 22 | max_velocity: 6.2832 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | joint_5: 26 | has_velocity_limits: true 27 | max_velocity: 6.2832 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | joint_6: 31 | has_velocity_limits: true 32 | max_velocity: 7.854 33 | has_acceleration_limits: false 34 | max_acceleration: 0 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 6 | manipulator_asus: 7 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 8 | kinematics_solver_search_resolution: 0.005 9 | kinematics_solver_timeout: 0.005 10 | kinematics_solver_attempts: 3 11 | manipulator_ensenso: 12 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 13 | kinematics_solver_search_resolution: 0.005 14 | kinematics_solver_timeout: 0.005 15 | kinematics_solver_attempts: 3 16 | manipulator_keyence: 17 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 18 | kinematics_solver_search_resolution: 0.005 19 | kinematics_solver_timeout: 0.005 20 | kinematics_solver_attempts: 3 21 | manipulator_tcp: 22 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 23 | kinematics_solver_search_resolution: 0.005 24 | kinematics_solver_timeout: 0.005 25 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/irb2400_calib_planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/irb2400_robot_ftp_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/irb2400_workspace_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/irb2400_workspace_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | godel_irb2400_moveit_config 3 | 0.2.0 4 | 5 | An automatically generated package with all the configuration and launch files for using the irb2400_workspace with the MoveIt Motion Planning Framework 6 | 7 | 8 | MoveIt Setup Assistant 9 | MoveIt Setup Assistant 10 | BSD 11 | http://moveit.ros.org/ 12 | https://github.com/ros-planning/moveit_setup_assistant/issues 13 | https://github.com/ros-planning/moveit_setup_assistant 14 | 15 | catkin 16 | 17 | moveit_ros_move_group 18 | moveit_planners_ompl 19 | moveit_ros_visualization 20 | joint_state_publisher 21 | robot_state_publisher 22 | xacro 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_irb2400_support) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY 9 | config 10 | launch 11 | rviz 12 | urdf 13 | yaml 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 15 | ) 16 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/config/blending_plan.yaml: -------------------------------------------------------------------------------- 1 | blending_plan: 2 | tool_radius: 0.0125 # (m) actual radius of blending tool 3 | margin: 0.001 # (m) additional distance to leave near surface boundaries 4 | overlap: 0.00 # (m) overlap distance between adjacent paths 5 | approach_spd: 0.005 # (m/s) Speed to approach surface of part 6 | blending_spd: 0.3 # (m/s) Speed to perform blending 7 | retract_spd: 0.02 # (m/s) Speed to move away from surface of part 8 | traverse_spd: 0.3 # (m/s) Speed to travel while at traverse height 9 | discretization: 0.0025 # (m) How densely to space adjacent Cartesian points 10 | safe_traverse_height: 0.05 # (m) height above surface to rise during rapid traversal moves 11 | min_boundary_length: .03 # (m) Boundaries below threshold are ignored during process path creation 12 | tool_force: 0.0 # (kg) 13 | spindle_speed: 0.0 # (rad/s) -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/config/plugins.yaml: -------------------------------------------------------------------------------- 1 | meshing_plugin_name: "concave_hull_mesher::ConcaveHullMesher" 2 | blend_tool_planning_plugin_name: "path_planning_plugins::openveronoi::BlendPlanner" 3 | scan_tool_planning_plugin_name: "path_planning_plugins::openveronoi::ScanPlanner" 4 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/config/point_cloud_descriptions.yaml: -------------------------------------------------------------------------------- 1 | frame_id: world_frame 2 | point_cloud_descriptions: 3 | - x: 0.95 4 | y: 0.2 5 | z: 0.36 6 | rx: 0.0 7 | ry: 0.0 8 | rz: 0.0 9 | l: 0.1 10 | w: 0.19 11 | h: 0.0 12 | resolution: 0.0005 13 | - x: 1.1 14 | y: -0.1 15 | z: 0.1 16 | rx: 0.0 17 | ry: 0.0 18 | rz: 0.0 19 | l: 0.2 20 | w: 0.1 21 | h: 0.0 22 | resolution: 0.0005 23 | - x: 0.7 24 | y: -0.5 25 | z: 0.3 26 | rx: 0.0 27 | ry: 0.0 28 | rz: 0.0 29 | l: 0.2 30 | w: 0.2 31 | h: 0.0 32 | resolution: 0.0005 33 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/config/robot_scan.yaml: -------------------------------------------------------------------------------- 1 | robot_scan: 2 | group_name: manipulator_asus 3 | home_position: home_asus 4 | world_frame: world_frame 5 | tcp_frame: kinect2_move_frame 6 | tcp_to_cam_pose: 7 | trans: 8 | x: 0 9 | y: 0 10 | z: 0 11 | quat: 12 | x: 0 13 | y: 0 14 | z: 0 15 | w: 1 16 | world_to_obj_pose: 17 | trans: 18 | x: 0.5 19 | y: 0.0 20 | z: 0.5 21 | quat: 22 | x: 0 23 | y: 0 24 | z: 0 25 | w: 1 26 | cam_to_obj_zoffset: 0.6 27 | cam_to_obj_xoffset: 0.5 28 | cam_tilt_angle: 1.57 29 | sweep_angle_start: 0 30 | sweep_angle_end: 0.25 31 | scan_topic: sensor_point_cloud 32 | scan_target_frame: world_frame 33 | reachable_scan_points_ratio: 1.0 34 | stop_on_planning_error: true 35 | num_scan_points: 3 36 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/config/scan_plan.yaml: -------------------------------------------------------------------------------- 1 | scan_plan: 2 | scan_width: 0.01 3 | traverse_speed: 0.2 4 | approach_distance: 0.15 5 | overlap: 0.001 6 | margin: 0 7 | quality_metric: "rms" 8 | window_width: 0.02 9 | min_qa_value: 0.05 10 | max_qa_value: 0.05 11 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/config/surface_detection.yaml: -------------------------------------------------------------------------------- 1 | surface_detection: 2 | frame_id: "world_frame" 3 | k_search: 20 4 | marker_alpha: 1 5 | 6 | meanK: 10 7 | stdv_threshold: 4 8 | 9 | rg_min_cluster_size: 100 10 | rg_max_cluster_size: 10000 11 | rg_neighbors: 20 12 | rg_smoothness_threshold: 0.07853981633974483 13 | rg_curvature_threshold: 2.0 14 | 15 | stout_mean: 1.0 16 | stout_stdev_threshold: 3.0 17 | 18 | voxel_leaf: 0.005 19 | tabletop_seg_distance_thresh: 0.005 20 | use_tabletop_segmentation: False 21 | ignore_largest_cluster: False 22 | 23 | mls_point_density: 100 24 | mls_upsampling_radius: 0.01 25 | mls_search_radius: 0.04 26 | 27 | pa_enabled: True 28 | pa_seg_max_iterations: 200 29 | pa_seg_dist_threshold: 0.01 30 | pa_sac_plane_distance: 0.01 31 | pa_kdtree_radius: 0.01 32 | 33 | use_octomap: False 34 | occupancy_threshold: 0.5 35 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_irb2400_support 4 | 0.1.0 5 | 6 | Support package for a Godel system using ABB2400. Includes blending and calibration files. 7 | 8 | 9 | Jonathan Meyer 10 | 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | godel_surface_detection 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/godel_irb2400_support/urdf/irb2400_workspace.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(irb2400_ikfast_manipulator_plugin) 3 | add_definitions(-std=c++11) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | moveit_core 7 | pluginlib 8 | roscpp 9 | tf_conversions 10 | ) 11 | 12 | include_directories(${catkin_INCLUDE_DIRS}) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS include 16 | LIBRARIES 17 | CATKIN_DEPENDS 18 | moveit_core 19 | pluginlib 20 | roscpp 21 | tf_conversions 22 | ) 23 | 24 | include_directories(include) 25 | 26 | set(IKFAST_LIBRARY_NAME godel_abb_irb2400_manipulator_moveit_ikfast_plugin) 27 | 28 | find_package(LAPACK REQUIRED) 29 | 30 | add_library(${IKFAST_LIBRARY_NAME} src/plugin_init.cpp) 31 | target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) 32 | 33 | install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 34 | 35 | install( 36 | FILES 37 | abb_irb2400_manipulator_moveit_ikfast_plugin_description.xml 38 | DESTINATION 39 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 40 | ) -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_moveit_ikfast_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | IKFast61 plugin for closed-form kinematics 5 | 6 | 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | irb2400_ikfast_manipulator_plugin 4 | 0.1.0 5 | 6 | IKFast plugin for the ABB2400; used for closed form IK in Godel 7 | 8 | 9 | Jonathan Meyer 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | liblapack-dev 15 | moveit_core 16 | pluginlib 17 | roscpp 18 | tf_conversions 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/src/plugin_init.cpp: -------------------------------------------------------------------------------- 1 | 2 | // register IKFastKinematicsPlugin as a KinematicsBase implementation 3 | #include 4 | #include 5 | PLUGINLIB_EXPORT_CLASS(irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin, 6 | kinematics::KinematicsBase); 7 | -------------------------------------------------------------------------------- /godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/update_ikfast_plugin.sh: -------------------------------------------------------------------------------- 1 | rosrun moveit_ikfast create_ikfast_moveit_plugin.py abb_irb2400 manipulator irb2400_ikfast_manipulator_plugin /home/ros-industrial/ros/abbikfast/src/irb2400_ikfast_manipulator_plugin/src/abb_irb2400_manipulator_ikfast_solver.cpp -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(blending_end_effector) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY 9 | config 10 | launch 11 | rviz 12 | urdf 13 | yaml 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 15 | ) 16 | -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/launch/blending_end_effector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/collision/EE_and_Tool_Changer_convex_hull.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/collision/EE_and_Tool_Changer_convex_hull.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/collision/adapter_tube_convex_hull.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/collision/adapter_tube_convex_hull.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/visual/Adapter_Tube.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/visual/Adapter_Tube.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/visual/Blending_Tool.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/visual/Blending_Tool.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/visual/Ensenso.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/visual/Ensenso.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/visual/Laser_Sensor.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/visual/Laser_Sensor.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/meshes/visual/Tool_Mounting_Plate.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/godel_robots/blending_end_effector/meshes/visual/Tool_Mounting_Plate.STL -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | blending_end_effector 4 | 0.1.0 5 | 6 | URDF robot description model for the blending end effector 7 | 8 | 9 | Jonathan Meyer 10 | 11 | Apache 2.0 12 | 13 | catkin 14 | 15 | industrial_robot_client 16 | 17 | 18 | -------------------------------------------------------------------------------- /godel_robots/blending_end_effector/urdf/blending_eef.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /godel_scan_analysis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_scan_analysis) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | pcl_ros 8 | roscpp 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS include 13 | CATKIN_DEPENDS 14 | pcl_ros 15 | roscpp 16 | ) 17 | 18 | include_directories( 19 | ${catkin_INCLUDE_DIRS} 20 | include 21 | ) 22 | 23 | add_executable(godel_scan_analysis_node 24 | src/godel_scan_analysis_node.cpp 25 | src/scan_roughness_scoring.cpp 26 | src/keyence_scan_server.cpp 27 | ) 28 | 29 | target_link_libraries(godel_scan_analysis_node 30 | ${catkin_LIBRARIES} 31 | ) 32 | 33 | install(TARGETS godel_scan_analysis_node 34 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 37 | ) 38 | -------------------------------------------------------------------------------- /godel_scan_analysis/include/godel_scan_analysis/scan_roughness_scoring.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_ROUGHNESS_SCORING_H 2 | #define SCAN_ROUGHNESS_SCORING_H 3 | 4 | #include 5 | #include 6 | 7 | namespace godel_scan_analysis 8 | { 9 | // TODO: add scoring params to this struct 10 | struct ScoringParams 11 | { 12 | }; 13 | 14 | /** 15 | * This class, once setup, scores individual laser scans 16 | * with a given algorithm, colorizes them, and pushes them 17 | * back out to a colorized cloud. 18 | */ 19 | class RoughnessScorer 20 | { 21 | public: 22 | // Input 23 | typedef pcl::PointCloud Cloud; 24 | // output 25 | typedef pcl::PointCloud ColorCloud; 26 | 27 | RoughnessScorer(); 28 | 29 | bool analyze(const Cloud& in, ColorCloud& out) const; 30 | 31 | private: 32 | ScoringParams params_; 33 | }; 34 | } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /godel_scan_analysis/include/godel_scan_analysis/scan_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_UTILITIES_H 2 | #define SCAN_UTILITIES_H 3 | 4 | #include 5 | 6 | namespace rms 7 | { 8 | 9 | template struct Point 10 | { 11 | FloatType x, y; 12 | }; 13 | 14 | template struct LineCoef 15 | { 16 | FloatType slope; 17 | FloatType intercept; 18 | 19 | LineCoef(double slope, double intercept) : slope(slope), intercept(intercept) {} 20 | 21 | FloatType operator()(const FloatType x) const { return slope * x + intercept; } 22 | }; 23 | 24 | template struct LineFitSums 25 | { 26 | FloatType x; 27 | FloatType y; 28 | FloatType x2; // sum of x*x 29 | FloatType xy; // sum of x*y 30 | std::size_t n; 31 | }; 32 | 33 | template struct Scan 34 | { 35 | typedef rms::Point value_type; 36 | std::vector points; 37 | }; 38 | 39 | typedef std::vector Scores; 40 | 41 | } // end namespace rms 42 | #endif 43 | -------------------------------------------------------------------------------- /godel_scan_analysis/launch/scan_analysis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /godel_scan_analysis/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_scan_analysis 4 | 0.0.0 5 | Generates colorized point clouds from individual profilimeter 6 | laser scans. 7 | 8 | Jon Meyer 9 | Apache 2.0 10 | 11 | catkin 12 | 13 | pcl_ros 14 | roscpp 15 | 16 | 17 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/blending_panel.h: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_GUI_BLENDING_PANEL_H 2 | #define SIMPLE_GUI_BLENDING_PANEL_H 3 | 4 | #include "rviz/panel.h" 5 | 6 | namespace godel_simple_gui 7 | { 8 | 9 | // Forward declare blend widget 10 | class BlendingWidget; 11 | 12 | class BlendingPanel : public rviz::Panel 13 | { 14 | Q_OBJECT 15 | public: 16 | BlendingPanel(QWidget* parent = 0); 17 | 18 | virtual ~BlendingPanel(); 19 | 20 | virtual void onInitialize(); 21 | 22 | protected: 23 | BlendingWidget* widget_; 24 | }; 25 | } 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/gui_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_GUI_STATE_H 2 | #define SIMPLE_GUI_STATE_H 3 | 4 | #include 5 | 6 | namespace godel_simple_gui 7 | { 8 | 9 | // Forward declare Main Widget 10 | class BlendingWidget; 11 | 12 | class GuiState : public QObject 13 | { 14 | Q_OBJECT 15 | public: 16 | virtual ~GuiState() {} 17 | 18 | // Entry and exit classes 19 | virtual void onStart(BlendingWidget& gui) = 0; 20 | virtual void onExit(BlendingWidget& gui) = 0; 21 | 22 | // Handlers for the fixed buttons 23 | virtual void onNext(BlendingWidget& gui) = 0; 24 | virtual void onBack(BlendingWidget& gui) = 0; 25 | virtual void onReset(BlendingWidget& gui) = 0; 26 | 27 | Q_SIGNALS: 28 | void newStateAvailable(GuiState*); 29 | }; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/options/path_planning_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_PLANNING_CONFIGURATION_H 2 | #define PATH_PLANNING_CONFIGURATION_H 3 | 4 | #include "godel_simple_gui/parameter_window_base.h" 5 | #include "godel_msgs/PathPlanningParameters.h" 6 | 7 | namespace Ui 8 | { 9 | class PathPlanningConfigWindow; 10 | } 11 | 12 | namespace godel_simple_gui 13 | { 14 | 15 | class PathPlanningConfigWidget : public ParameterWindowBase 16 | { 17 | Q_OBJECT 18 | public: 19 | PathPlanningConfigWidget(godel_msgs::PathPlanningParameters params); 20 | 21 | godel_msgs::PathPlanningParameters& params() { return params_; } 22 | 23 | virtual void update_display_fields(); 24 | 25 | virtual void update_internal_fields(); 26 | 27 | protected: 28 | godel_msgs::PathPlanningParameters params_; 29 | Ui::PathPlanningConfigWindow* ui_; 30 | }; 31 | } 32 | 33 | #endif // PATH_PLANNING_CONFIGURATION_H 34 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/options/pose_widget.h: -------------------------------------------------------------------------------- 1 | #ifndef GODEL_SIMPLE_GUI_POSE_WIDGET 2 | #define GODEL_SIMPLE_GUI_POSE_WIDGET 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace Ui 10 | { 11 | class PoseWidget; 12 | } 13 | 14 | namespace godel_simple_gui 15 | { 16 | 17 | class PoseWidget : public QWidget 18 | { 19 | Q_OBJECT 20 | public: 21 | PoseWidget(QWidget* parent = NULL); 22 | 23 | void set_values(const geometry_msgs::Pose& p); 24 | void set_values(const tf::Transform& t); 25 | tf::Transform get_values(); 26 | 27 | protected: 28 | Ui::PoseWidget* ui_; 29 | }; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/options/robot_scan_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_SCAN_CONFIGURATION_H 2 | #define ROBOT_SCAN_CONFIGURATION_H 3 | 4 | #include "godel_simple_gui/parameter_window_base.h" 5 | #include "godel_simple_gui/options/pose_widget.h" 6 | 7 | #include "godel_msgs/RobotScanParameters.h" 8 | 9 | namespace Ui 10 | { 11 | class RobotScanConfigWindow; 12 | } 13 | 14 | namespace godel_simple_gui 15 | { 16 | 17 | class RobotScanConfigWidget : public ParameterWindowBase 18 | { 19 | Q_OBJECT 20 | public: 21 | RobotScanConfigWidget(godel_msgs::RobotScanParameters params); 22 | 23 | virtual ~RobotScanConfigWidget(); 24 | 25 | godel_msgs::RobotScanParameters& params() { return params_; } 26 | const godel_msgs::RobotScanParameters& params() const { return params_; } 27 | 28 | virtual void update_display_fields(); 29 | virtual void update_internal_fields(); 30 | 31 | protected: 32 | godel_msgs::RobotScanParameters params_; 33 | PoseWidget* world_to_obj_pose_widget_; 34 | PoseWidget* tcp_to_cam_pose_widget_; 35 | Ui::RobotScanConfigWindow* ui_; 36 | }; 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/options/scan_tool_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_TOOL_CONFIGURATION_H 2 | #define SCAN_TOOL_CONFIGURATION_H 3 | 4 | #include "godel_simple_gui/parameter_window_base.h" 5 | 6 | #include "godel_msgs/ScanPlanParameters.h" 7 | 8 | namespace Ui 9 | { 10 | class ScanToolConfigWindow; 11 | } 12 | 13 | namespace godel_simple_gui 14 | { 15 | 16 | class ScanPlanConfigWidget : public ParameterWindowBase 17 | { 18 | Q_OBJECT 19 | public: 20 | ScanPlanConfigWidget(godel_msgs::ScanPlanParameters params); 21 | godel_msgs::ScanPlanParameters& params() { return params_; } 22 | 23 | virtual void update_display_fields(); 24 | virtual void update_internal_fields(); 25 | 26 | static QStringList quality_metric_list; 27 | 28 | protected: 29 | // Maps scan-method enum to a field inside the combo-box display 30 | virtual int get_quality_combobox_index(); 31 | 32 | // Maps a field in combo-box display to scan-method enum 33 | virtual int get_scan_method_enum_value(); 34 | 35 | godel_msgs::ScanPlanParameters params_; 36 | Ui::ScanToolConfigWindow* ui_; 37 | }; 38 | } 39 | 40 | #endif // SCAN_TOOL_CONFIGURATION_H 41 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/options/surface_detection_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef SURFACE_DETECTION_CONFIGURATION_H 2 | #define SURFACE_DETECTION_CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace Ui 9 | { 10 | class SurfaceDetectionConfigWindow; 11 | } 12 | 13 | namespace godel_simple_gui 14 | { 15 | 16 | class SurfaceDetectionConfigWidget : public ParameterWindowBase 17 | { 18 | Q_OBJECT 19 | public: 20 | SurfaceDetectionConfigWidget(godel_msgs::SurfaceDetectionParameters params); 21 | 22 | godel_msgs::SurfaceDetectionParameters& params() { return params_; } 23 | const godel_msgs::SurfaceDetectionParameters& params() const { return params_; } 24 | 25 | virtual void update_display_fields(); 26 | 27 | virtual void update_internal_fields(); 28 | 29 | protected: 30 | godel_msgs::SurfaceDetectionParameters params_; 31 | Ui::SurfaceDetectionConfigWindow* ui_; 32 | }; 33 | } 34 | 35 | #endif // SURFACE_DETECTION_CONFIGURATION_H 36 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/options_submenu.h: -------------------------------------------------------------------------------- 1 | #ifndef OPTIONS_SUBMENU_H 2 | #define OPTIONS_SUBMENU_H 3 | 4 | #include 5 | 6 | #include "godel_simple_gui/options/robot_scan_configuration.h" 7 | #include "godel_simple_gui/options/surface_detection_configuration.h" 8 | #include "godel_simple_gui/options/scan_tool_configuration.h" 9 | #include "godel_simple_gui/options/path_planning_configuration.h" 10 | 11 | namespace Ui 12 | { 13 | class OptionsSubmenu; 14 | } 15 | 16 | namespace godel_simple_gui 17 | { 18 | 19 | class OptionsSubmenu : public QWidget 20 | { 21 | Q_OBJECT 22 | 23 | public: 24 | OptionsSubmenu(QWidget* parent = 0); 25 | 26 | // For every submenu / set of parameters, we have getters/setters 27 | const godel_msgs::RobotScanParameters& robotScanParams() const; 28 | void setRobotScanParams(const godel_msgs::RobotScanParameters& params); 29 | 30 | const godel_msgs::SurfaceDetectionParameters& surfaceDetectionParams() const; 31 | void setSurfaceDetectionParams(const godel_msgs::SurfaceDetectionParameters& params); 32 | 33 | const godel_msgs::PathPlanningParameters& pathPlanningParams() const; 34 | void setPathPlanningParams(const godel_msgs::PathPlanningParameters& params); 35 | 36 | const godel_msgs::ScanPlanParameters& scanParams() const; 37 | void setScanParams(const godel_msgs::ScanPlanParameters& params); 38 | 39 | Q_SIGNALS: 40 | void saveRequested(); 41 | 42 | private: 43 | // Display layout 44 | Ui::OptionsSubmenu* ui_; 45 | // Configuration components 46 | RobotScanConfigWidget* robot_scan_; 47 | SurfaceDetectionConfigWidget* surface_detection_; 48 | PathPlanningConfigWidget* path_planning_params_; 49 | ScanPlanConfigWidget* scan_params_; 50 | }; 51 | 52 | } // end namespace godel_simple_gui 53 | 54 | #endif // OPTIONS_SUBMENU_H 55 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/parameter_window_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETER_WINDOW_BASE_H 2 | #define PARAMETER_WINDOW_BASE_H 3 | 4 | #include 5 | 6 | namespace godel_simple_gui 7 | { 8 | 9 | /** 10 | * @brief Abstract base class for parameter windows in the blending widget 11 | */ 12 | class ParameterWindowBase : public QWidget 13 | { 14 | Q_OBJECT 15 | 16 | public: 17 | /** 18 | * @brief Updates GUI parameters and shows the main window 19 | */ 20 | virtual void show(); 21 | 22 | /** 23 | * @brief Reads the internal data structure to update the fields of the GUI 24 | */ 25 | virtual void update_display_fields() = 0; 26 | 27 | /** 28 | * @brief Reads the fields of the GUI to update the internal data structure 29 | */ 30 | virtual void update_internal_fields() = 0; 31 | 32 | Q_SIGNALS: 33 | /** 34 | * @brief Indicates an edit of the GUI interface by the user 35 | */ 36 | void parameters_changed(); 37 | void parameters_save_requested(); 38 | 39 | protected Q_SLOTS: 40 | 41 | virtual void accept_changes_handler(); 42 | virtual void cancel_changes_handler(); 43 | virtual void save_changes_handler(); 44 | }; 45 | } 46 | 47 | #endif // PARAMETER_WINDOW_BASE_H 48 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/error_state.h: -------------------------------------------------------------------------------- 1 | #ifndef ERROR_STATE_H 2 | #define ERROR_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | namespace godel_simple_gui 8 | { 9 | 10 | class ErrorState : public GuiState 11 | { 12 | Q_OBJECT 13 | public: 14 | ErrorState(const std::string& msg, GuiState* next_state); 15 | 16 | // Entry and exit classes 17 | virtual void onStart(BlendingWidget& gui); 18 | virtual void onExit(BlendingWidget& gui); 19 | 20 | // Handlers for the fixed buttons 21 | virtual void onNext(BlendingWidget& gui); 22 | virtual void onBack(BlendingWidget& gui); 23 | virtual void onReset(BlendingWidget& gui); 24 | 25 | private: 26 | std::string msg_; 27 | GuiState* next_state_; 28 | QWidget* window_; 29 | }; 30 | } 31 | 32 | #endif // ERROR_STATE_H 33 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/executing_state.h: -------------------------------------------------------------------------------- 1 | #ifndef EXECUTING_STATE_H 2 | #define EXECUTING_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | namespace godel_simple_gui 8 | { 9 | 10 | class ExecutingState : public GuiState 11 | { 12 | Q_OBJECT 13 | public: 14 | // Constructor 15 | ExecutingState(const std::vector& plans); 16 | 17 | // Entry and exit classes 18 | virtual void onStart(BlendingWidget& gui); 19 | virtual void onExit(BlendingWidget& gui); 20 | 21 | // Handlers for the fixed buttons 22 | virtual void onNext(BlendingWidget& gui); 23 | virtual void onBack(BlendingWidget& gui); 24 | virtual void onReset(BlendingWidget& gui); 25 | 26 | protected: 27 | void executeOne(const std::string &plan, BlendingWidget& gui); 28 | void executeAll(BlendingWidget& gui); 29 | 30 | private: 31 | std::vector plan_names_; 32 | ros::ServiceClient real_client_; 33 | }; 34 | } 35 | 36 | #endif // EXECUTING_STATE_H 37 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/planning_state.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANNING_STATE_H 2 | #define PLANNING_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | const static std::string PROCESS_PLANNING_ACTION_SERVER_NAME = "process_planning_as"; 12 | 13 | namespace godel_simple_gui 14 | { 15 | 16 | class PlanningState : public GuiState 17 | { 18 | Q_OBJECT 19 | private: 20 | ros::NodeHandle nh_; 21 | actionlib::SimpleActionClient process_planning_action_client_; 22 | BlendingWidget* gui_ptr_; 23 | 24 | void makeRequest(godel_msgs::PathPlanningParameters params); 25 | void processPlanningDoneCallback(const actionlib::SimpleClientGoalState& state, 26 | const godel_msgs::ProcessPlanningResultConstPtr& result); 27 | void processPlanningActiveCallback(); 28 | void processPlanningFeedbackCallback(const godel_msgs::ProcessPlanningFeedbackConstPtr& feedback); 29 | 30 | Q_SIGNALS: 31 | void feedbackReceived(QString feedback); 32 | 33 | protected Q_SLOTS: 34 | void setFeedbackText(QString feedback); 35 | 36 | public: 37 | PlanningState(); 38 | // Entry and exit classes 39 | virtual void onStart(BlendingWidget& gui); 40 | virtual void onExit(BlendingWidget& gui); 41 | 42 | // Handlers for the fixed buttons 43 | virtual void onNext(BlendingWidget& gui); 44 | virtual void onBack(BlendingWidget& gui); 45 | virtual void onReset(BlendingWidget& gui); 46 | }; 47 | } 48 | 49 | #endif // PLANNING_STATE_H 50 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/scan_teach_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SCAN_TEACH_STATE_H 2 | #define SCAN_TEACH_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | namespace godel_simple_gui 8 | { 9 | 10 | class ScanTeachState : public GuiState 11 | { 12 | Q_OBJECT 13 | public: 14 | // Entry and exit classes 15 | virtual void onStart(BlendingWidget& gui); 16 | virtual void onExit(BlendingWidget& gui); 17 | 18 | // Handlers for the fixed buttons 19 | virtual void onNext(BlendingWidget& gui); 20 | virtual void onBack(BlendingWidget& gui); 21 | virtual void onReset(BlendingWidget& gui); 22 | }; 23 | } 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/scanning_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SCANNING_STATE_H 2 | #define SCANNING_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | 6 | #include 7 | 8 | #include "godel_msgs/RobotScanParameters.h" 9 | #include "godel_msgs/SurfaceDetectionParameters.h" 10 | 11 | namespace godel_simple_gui 12 | { 13 | class ScanningState : public GuiState 14 | { 15 | Q_OBJECT 16 | public: 17 | // Entry and exit classes 18 | virtual void onStart(BlendingWidget& gui); 19 | virtual void onExit(BlendingWidget& gui); 20 | 21 | // Handlers for the fixed buttons 22 | virtual void onNext(BlendingWidget& gui); 23 | virtual void onBack(BlendingWidget& gui); 24 | virtual void onReset(BlendingWidget& gui); 25 | 26 | private: 27 | void makeRequest(godel_msgs::RobotScanParameters scan_params, 28 | godel_msgs::SurfaceDetectionParameters surface_params); 29 | 30 | ros::ServiceClient surface_client_; 31 | }; 32 | } 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/select_all_surface_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SELECT_ALL_SURFACE_STATE_H 2 | #define SELECT_ALL_SURFACE_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | 6 | namespace godel_simple_gui 7 | { 8 | 9 | class SelectAllSurfaceState : public GuiState 10 | { 11 | public: 12 | // Entry and exit classes 13 | virtual void onStart(BlendingWidget& gui); 14 | virtual void onExit(BlendingWidget& gui); 15 | 16 | // Handlers for the fixed buttons 17 | virtual void onNext(BlendingWidget& gui); 18 | virtual void onBack(BlendingWidget& gui); 19 | virtual void onReset(BlendingWidget& gui); 20 | }; 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/select_plans_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SELECT_PLANS_STATE_H 2 | #define SELECT_PLANS_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | namespace godel_simple_gui 8 | { 9 | 10 | class SelectPlansState : public GuiState 11 | { 12 | Q_OBJECT 13 | public: 14 | // Entry and exit classes 15 | virtual void onStart(BlendingWidget& gui); 16 | virtual void onExit(BlendingWidget& gui); 17 | 18 | // Handlers for the fixed buttons 19 | virtual void onNext(BlendingWidget& gui); 20 | virtual void onBack(BlendingWidget& gui); 21 | virtual void onReset(BlendingWidget& gui); 22 | 23 | private: 24 | std::vector plan_names_; 25 | }; 26 | } 27 | 28 | #endif // WAIT_TO_SIMULATE_STATE_H 29 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/simulating_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SIMULATING_STATE_H 2 | #define SIMULATING_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | namespace godel_simple_gui 8 | { 9 | 10 | class SimulatingState : public GuiState 11 | { 12 | Q_OBJECT 13 | public: 14 | // Constructor 15 | SimulatingState(const std::vector& plans); 16 | 17 | // Entry and exit classes 18 | virtual void onStart(BlendingWidget& gui); 19 | virtual void onExit(BlendingWidget& gui); 20 | 21 | // Handlers for the fixed buttons 22 | virtual void onNext(BlendingWidget& gui); 23 | virtual void onBack(BlendingWidget& gui); 24 | virtual void onReset(BlendingWidget& gui); 25 | 26 | protected: 27 | void simulateAll(BlendingWidget& gui); 28 | void simulateOne(const std::string& plan, BlendingWidget& gui); 29 | 30 | private: 31 | std::vector plan_names_; 32 | ros::ServiceClient sim_client_; 33 | }; 34 | } 35 | 36 | #endif // SIMULATING_STATE_H 37 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/surface_select_state.h: -------------------------------------------------------------------------------- 1 | #ifndef SURFACE_SELECT_STATE_H 2 | #define SURFACE_SELECT_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | 6 | namespace godel_simple_gui 7 | { 8 | 9 | class SurfaceSelectState : public GuiState 10 | { 11 | public: 12 | // Entry and exit classes 13 | virtual void onStart(BlendingWidget& gui); 14 | virtual void onExit(BlendingWidget& gui); 15 | 16 | // Handlers for the fixed buttons 17 | virtual void onNext(BlendingWidget& gui); 18 | virtual void onBack(BlendingWidget& gui); 19 | virtual void onReset(BlendingWidget& gui); 20 | }; 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /godel_simple_gui/include/godel_simple_gui/states/wait_to_execute_state.h: -------------------------------------------------------------------------------- 1 | #ifndef WAIT_TO_EXECUTE_STATE_H 2 | #define WAIT_TO_EXECUTE_STATE_H 3 | 4 | #include "godel_simple_gui/gui_state.h" 5 | #include 6 | 7 | namespace godel_simple_gui 8 | { 9 | 10 | class WaitToExecuteState : public GuiState 11 | { 12 | Q_OBJECT 13 | public: 14 | WaitToExecuteState(const std::vector& plans); 15 | 16 | // Entry and exit classes 17 | virtual void onStart(BlendingWidget& gui); 18 | virtual void onExit(BlendingWidget& gui); 19 | 20 | // Handlers for the fixed buttons 21 | virtual void onNext(BlendingWidget& gui); 22 | virtual void onBack(BlendingWidget& gui); 23 | virtual void onReset(BlendingWidget& gui); 24 | 25 | private: 26 | std::vector plan_names_; 27 | }; 28 | } 29 | #endif // WAIT_FOR_EXECUTE_STATE_H 30 | -------------------------------------------------------------------------------- /godel_simple_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_simple_gui 4 | 0.1.0 5 | The godel_simple_gui package presents an alternative front end for the Godel project that is focused on providing an easy to use, ATM-like interface for operating the system end to end. 6 | 7 | Jonathan Meyer 8 | Jonathan Meyer 9 | Apache 2.0 10 | 11 | catkin 12 | 13 | godel_msgs 14 | roscpp 15 | rviz 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /godel_simple_gui/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | A panel widget that can be used in an rviz instance for surface blending path generation 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /godel_simple_gui/src/blending_panel.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/blending_panel.h" 2 | 3 | #include 4 | 5 | #include "godel_simple_gui/blending_widget.h" 6 | 7 | #include 8 | 9 | godel_simple_gui::BlendingPanel::BlendingPanel(QWidget* parent) : rviz::Panel(parent) 10 | { 11 | ROS_INFO("Loaded simple blending panel"); 12 | 13 | QVBoxLayout* layout = new QVBoxLayout(this); 14 | widget_ = new BlendingWidget(); 15 | layout->addWidget(widget_); 16 | setLayout(layout); 17 | } 18 | 19 | godel_simple_gui::BlendingPanel::~BlendingPanel() {} 20 | 21 | void godel_simple_gui::BlendingPanel::onInitialize() 22 | { 23 | ROS_INFO("Initializng simple blending panel"); 24 | } 25 | 26 | #include 27 | PLUGINLIB_EXPORT_CLASS(godel_simple_gui::BlendingPanel, rviz::Panel) 28 | -------------------------------------------------------------------------------- /godel_simple_gui/src/options/path_planning_configuration.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/options/path_planning_configuration.h" 2 | 3 | #include "ui_path_planning_configuration.h" 4 | 5 | godel_simple_gui::PathPlanningConfigWidget::PathPlanningConfigWidget( 6 | godel_msgs::PathPlanningParameters params) 7 | : params_(params) 8 | { 9 | ui_ = new Ui::PathPlanningConfigWindow(); 10 | ui_->setupUi(this); 11 | 12 | connect(ui_->PushButtonAccept, SIGNAL(clicked()), this, SLOT(accept_changes_handler())); 13 | connect(ui_->PushButtonCancel, SIGNAL(clicked()), this, SLOT(cancel_changes_handler())); 14 | connect(ui_->PushButtonSave, SIGNAL(clicked()), this, SLOT(save_changes_handler())); 15 | } 16 | 17 | void godel_simple_gui::PathPlanningConfigWidget::update_display_fields() 18 | { 19 | ui_->LineEditDiscretization->setText(QString::number(params_.discretization)); 20 | ui_->LineEditMargin->setText(QString::number(params_.margin)); 21 | ui_->LineEditOverlap->setText(QString::number(params_.overlap)); 22 | ui_->LineEditScanWidth->setText(QString::number(params_.scan_width)); 23 | ui_->LineEditToolRadius->setText(QString::number(params_.tool_radius)); 24 | ui_->LineEditTraverseHeight->setText(QString::number(params_.traverse_height)); 25 | 26 | } 27 | 28 | void godel_simple_gui::PathPlanningConfigWidget::update_internal_fields() 29 | { 30 | params_.discretization = ui_->LineEditDiscretization->text().toDouble(); 31 | params_.margin = ui_->LineEditMargin->text().toDouble(); 32 | params_.overlap = ui_->LineEditOverlap->text().toDouble(); 33 | params_.scan_width = ui_->LineEditScanWidth->text().toDouble(); 34 | params_.tool_radius = ui_->LineEditToolRadius->text().toDouble(); 35 | params_.traverse_height = ui_->LineEditTraverseHeight->text().toDouble(); 36 | } 37 | -------------------------------------------------------------------------------- /godel_simple_gui/src/options/pose_widget.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/options/pose_widget.h" 2 | #include "ui_pose_widget.h" 3 | 4 | #ifndef DEG2RAD 5 | #define DEG2RAD(x) ((x)*0.017453293) 6 | #endif 7 | 8 | #ifndef RAD2DEG 9 | #define RAD2DEG(x) ((x)*57.29578) 10 | #endif 11 | 12 | // Pose Widget 13 | godel_simple_gui::PoseWidget::PoseWidget(QWidget* parent) : QWidget(parent) 14 | { 15 | ui_ = new Ui::PoseWidget(); 16 | ui_->setupUi(this); 17 | set_values(tf::Transform::getIdentity()); 18 | } 19 | 20 | void godel_simple_gui::PoseWidget::set_values(const geometry_msgs::Pose& p) 21 | { 22 | tf::Transform t; 23 | tf::poseMsgToTF(p, t); 24 | set_values(t); 25 | } 26 | 27 | void godel_simple_gui::PoseWidget::set_values(const tf::Transform& t) 28 | { 29 | tf::Vector3 p = t.getOrigin(); 30 | tfScalar rx, ry, rz; 31 | t.getBasis().getRPY(rx, ry, rz, 1); 32 | ui_->LineEditX->setText(QString::number(p.x())); 33 | ui_->LineEditY->setText(QString::number(p.y())); 34 | ui_->LineEditZ->setText(QString::number(p.z())); 35 | ui_->LineEditRx->setText(QString::number(RAD2DEG(rx))); 36 | ui_->LineEditRy->setText(QString::number(RAD2DEG(ry))); 37 | ui_->LineEditRz->setText(QString::number(RAD2DEG(rz))); 38 | } 39 | 40 | tf::Transform godel_simple_gui::PoseWidget::get_values() 41 | { 42 | double x, y, z, rx, ry, rz; 43 | x = ui_->LineEditX->text().toDouble(); 44 | y = ui_->LineEditY->text().toDouble(); 45 | z = ui_->LineEditZ->text().toDouble(); 46 | rx = DEG2RAD(ui_->LineEditRx->text().toDouble()); 47 | ry = DEG2RAD(ui_->LineEditRy->text().toDouble()); 48 | rz = DEG2RAD(ui_->LineEditRz->text().toDouble()); 49 | 50 | // create transform 51 | tf::Vector3 p(x, y, z); 52 | tf::Quaternion q; 53 | q.setRPY(rx, ry, rz); 54 | 55 | return tf::Transform(q, p); 56 | } -------------------------------------------------------------------------------- /godel_simple_gui/src/parameter_window_base.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/parameter_window_base.h" 2 | #include 3 | 4 | void godel_simple_gui::ParameterWindowBase::show() 5 | { 6 | update_display_fields(); 7 | QWidget::show(); 8 | } 9 | 10 | void godel_simple_gui::ParameterWindowBase::accept_changes_handler() 11 | { 12 | this->update_internal_fields(); 13 | Q_EMIT parameters_changed(); 14 | hide(); 15 | } 16 | 17 | void godel_simple_gui::ParameterWindowBase::cancel_changes_handler() { hide(); } 18 | 19 | void godel_simple_gui::ParameterWindowBase::save_changes_handler() 20 | { 21 | accept_changes_handler(); 22 | Q_EMIT parameters_save_requested(); 23 | } 24 | -------------------------------------------------------------------------------- /godel_simple_gui/src/states/error_state.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/states/error_state.h" 2 | 3 | #include 4 | #include "godel_simple_gui/blending_widget.h" 5 | 6 | #include 7 | 8 | godel_simple_gui::ErrorState::ErrorState(const std::string& msg, GuiState* next_state) 9 | : msg_(msg), next_state_(next_state) 10 | { 11 | } 12 | 13 | void godel_simple_gui::ErrorState::onStart(BlendingWidget& gui) 14 | { 15 | gui.setText("An error was detected, please acknowledge."); 16 | gui.setButtonsEnabled(false); 17 | 18 | // Show error dialog 19 | window_ = new QWidget(); 20 | 21 | QMessageBox::StandardButton reply; 22 | reply = 23 | QMessageBox::critical(window_, tr("Warning"), QString::fromStdString(msg_), QMessageBox::Ok); 24 | Q_EMIT newStateAvailable(next_state_); 25 | } 26 | 27 | void godel_simple_gui::ErrorState::onExit(BlendingWidget& gui) 28 | { 29 | delete window_; 30 | gui.setButtonsEnabled(true); 31 | } 32 | 33 | // Handlers for the fixed buttons 34 | void godel_simple_gui::ErrorState::onNext(BlendingWidget& gui) {} 35 | 36 | void godel_simple_gui::ErrorState::onBack(BlendingWidget& gui) {} 37 | 38 | void godel_simple_gui::ErrorState::onReset(BlendingWidget& gui) {} 39 | -------------------------------------------------------------------------------- /godel_simple_gui/src/states/scan_teach_state.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/states/scan_teach_state.h" 2 | #include "godel_simple_gui/states/scanning_state.h" 3 | 4 | #include 5 | #include "godel_simple_gui/blending_widget.h" 6 | 7 | void godel_simple_gui::ScanTeachState::onStart(BlendingWidget& gui) 8 | { 9 | gui.setText("Ready to Scan\nPress Next to Continue"); 10 | gui.showStatusWindow(); 11 | } 12 | 13 | void godel_simple_gui::ScanTeachState::onExit(BlendingWidget& gui) {} 14 | 15 | void godel_simple_gui::ScanTeachState::onNext(BlendingWidget& gui) 16 | { 17 | Q_EMIT newStateAvailable(new ScanningState()); 18 | } 19 | 20 | void godel_simple_gui::ScanTeachState::onBack(BlendingWidget& gui) {} 21 | 22 | void godel_simple_gui::ScanTeachState::onReset(BlendingWidget& gui) {} 23 | -------------------------------------------------------------------------------- /godel_simple_gui/src/states/select_all_surface_state.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "godel_msgs/SelectSurface.h" 3 | #include "godel_simple_gui/blending_widget.h" 4 | #include "godel_simple_gui/states/select_all_surface_state.h" 5 | #include "godel_simple_gui/states/scan_teach_state.h" 6 | #include "godel_simple_gui/states/planning_state.h" 7 | 8 | void godel_simple_gui::SelectAllSurfaceState::onStart(BlendingWidget& gui) 9 | { 10 | const std::string SELECT_SURFACE_SERVICE = "select_surface"; 11 | ros::ServiceClient client = gui.nodeHandle().serviceClient( 12 | SELECT_SURFACE_SERVICE); 13 | godel_msgs::SelectSurface msg; 14 | msg.request.action = msg.request.SELECT_ALL; 15 | client.call(msg.request, msg.response); 16 | gui.setText("Press Next to select all surfaces.\nPress Back to select individual surfaces."); 17 | } 18 | 19 | void godel_simple_gui::SelectAllSurfaceState::onExit(BlendingWidget& gui) {} 20 | 21 | void godel_simple_gui::SelectAllSurfaceState::onNext(BlendingWidget& gui) 22 | { 23 | Q_EMIT newStateAvailable(new PlanningState()); 24 | } 25 | 26 | void godel_simple_gui::SelectAllSurfaceState::onBack(BlendingWidget& gui) 27 | { 28 | Q_EMIT newStateAvailable(new ScanTeachState()); 29 | } 30 | 31 | void godel_simple_gui::SelectAllSurfaceState::onReset(BlendingWidget& gui) 32 | { 33 | Q_EMIT newStateAvailable(new ScanTeachState()); 34 | } 35 | -------------------------------------------------------------------------------- /godel_simple_gui/src/states/surface_select_state.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/states/surface_select_state.h" 2 | #include "godel_simple_gui/states/scan_teach_state.h" 3 | #include "godel_simple_gui/states/planning_state.h" 4 | 5 | #include 6 | #include "godel_simple_gui/blending_widget.h" 7 | 8 | void godel_simple_gui::SurfaceSelectState::onStart(BlendingWidget& gui) 9 | { 10 | gui.setText("Pick Surfaces With Mouse\nPress Next to Continue"); 11 | } 12 | 13 | void godel_simple_gui::SurfaceSelectState::onExit(BlendingWidget& gui) {} 14 | 15 | // Handlers for the fixed buttons 16 | void godel_simple_gui::SurfaceSelectState::onNext(BlendingWidget& gui) 17 | { 18 | Q_EMIT newStateAvailable(new PlanningState()); 19 | } 20 | 21 | void godel_simple_gui::SurfaceSelectState::onBack(BlendingWidget& gui) 22 | { 23 | Q_EMIT newStateAvailable(new ScanTeachState()); 24 | } 25 | 26 | void godel_simple_gui::SurfaceSelectState::onReset(BlendingWidget& gui) 27 | { 28 | Q_EMIT newStateAvailable(new ScanTeachState()); 29 | } 30 | -------------------------------------------------------------------------------- /godel_simple_gui/src/states/wait_to_execute_state.cpp: -------------------------------------------------------------------------------- 1 | #include "godel_simple_gui/states/wait_to_execute_state.h" 2 | // previous state 3 | #include "godel_simple_gui/states/select_plans_state.h" 4 | // next state 5 | #include "godel_simple_gui/states/executing_state.h" 6 | // reset state 7 | #include "godel_simple_gui/states/scan_teach_state.h" 8 | 9 | #include 10 | #include "godel_simple_gui/blending_widget.h" 11 | 12 | godel_simple_gui::WaitToExecuteState::WaitToExecuteState(const std::vector& plans) 13 | : plan_names_(plans) 14 | { 15 | } 16 | 17 | void godel_simple_gui::WaitToExecuteState::onStart(BlendingWidget& gui) 18 | { 19 | ROS_INFO_STREAM("WaitToExecuteState start"); 20 | gui.setText("Ready to Execute.\nPress 'Next' to begin."); 21 | } 22 | 23 | void godel_simple_gui::WaitToExecuteState::onExit(BlendingWidget& gui) 24 | { 25 | ROS_INFO_STREAM("WaitToExecuteState exit"); 26 | } 27 | 28 | // Handlers for the fixed buttons 29 | void godel_simple_gui::WaitToExecuteState::onNext(BlendingWidget& gui) 30 | { 31 | ROS_INFO_STREAM("WaitToExecuteState next"); 32 | Q_EMIT newStateAvailable(new ExecutingState(plan_names_)); 33 | } 34 | 35 | void godel_simple_gui::WaitToExecuteState::onBack(BlendingWidget& gui) 36 | { 37 | ROS_INFO_STREAM("WaitToExecuteState back"); 38 | Q_EMIT newStateAvailable(new SelectPlansState()); 39 | } 40 | 41 | void godel_simple_gui::WaitToExecuteState::onReset(BlendingWidget& gui) 42 | { 43 | ROS_INFO_STREAM("WaitToExecuteState reset"); 44 | Q_EMIT newStateAvailable(new ScanTeachState()); 45 | } 46 | -------------------------------------------------------------------------------- /godel_surface_detection/include/services/trajectory_library.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORY_LIBRARY_H 2 | #define TRAJECTORY_LIBRARY_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace godel_surface_detection 10 | { 11 | 12 | class TrajectoryLibrary 13 | { 14 | public: 15 | typedef std::map TrajectoryMap; 16 | 17 | void load(const std::string& filename); 18 | void save(const std::string& filename); 19 | 20 | TrajectoryMap& get() { return map_; } 21 | const TrajectoryMap& get() const { return map_; } 22 | 23 | private: 24 | TrajectoryMap map_; 25 | }; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /godel_surface_detection/include/utils/mesh_conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef GODEL_MESH_CONVERSIONS_H 2 | #define GODEL_MESH_CONVERSIONS_H 3 | 4 | #include 5 | #include 6 | 7 | namespace godel_surface_detection 8 | { 9 | 10 | /** 11 | * @brief Flattens the list of vertices and polygon-indices found in the pcl::PolygonMesh data 12 | * structure into a simple list of 3d positions. Meant for serializing mesh data into a 13 | * visualization_msgs::Marker TRIANGLE_LIST. 14 | * @param mesh Source of our vertex and index data. We currently only handle 3 and 4 sided polygons. 15 | * @param points Output parameter where each sequential tuple of 3 points is a triangle. 16 | */ 17 | void meshToTrianglePoints(const pcl::PolygonMesh& mesh, std::vector& points); 18 | 19 | /** 20 | * @brief Converts a flattened triangle list into a pcl::PolygonMesh object. DOES NOT attempt 21 | * to reduce the vertex count. 22 | * @param points A sequence of points where each sequential set of 3 points is a triangle. 23 | * @param mesh A polygon mesh of the triangles in \e points. If any triangles are connected, 24 | * this will produce duplicate points. 25 | */ 26 | void trianglePointsToMesh(const std::vector& points, pcl::PolygonMesh& mesh); 27 | 28 | } 29 | 30 | #endif // GODEL_MESH_CONVERSIONS_H 31 | -------------------------------------------------------------------------------- /godel_surface_detection/launch/godel_core.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /godel_surface_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_surface_detection 4 | 0.1.0 5 | 6 | 7 | This package houses the primary blending "server" that is responsible for 8 | making the whole system go. It is actuated via service calls and farms out 9 | jobs to the many planning and execution components of the Godel project. 10 | 11 | 12 | jnicho 13 | Jonathan Meyer 14 | 15 | Apache 2.0 16 | 17 | catkin 18 | 19 | pcl_msgs 20 | pcl_ros 21 | roscpp 22 | sensor_msgs 23 | interactive_markers 24 | moveit_ros_planning_interface 25 | tf 26 | tf_conversions 27 | godel_msgs 28 | godel_process_path_generation 29 | godel_param_helpers 30 | godel_plugins 31 | godel_utils 32 | meshing_plugins_base 33 | path_planning_plugins_base 34 | swri_profiler 35 | 36 | moveit_ros_move_group 37 | 38 | 39 | -------------------------------------------------------------------------------- /godel_surface_detection/src/services/trajectory_library.cpp: -------------------------------------------------------------------------------- 1 | #include "services/trajectory_library.h" 2 | 3 | #include 4 | #include 5 | 6 | void godel_surface_detection::TrajectoryLibrary::save(const std::string& filename) 7 | { 8 | rosbag::Bag bag; 9 | bag.open(filename, rosbag::bagmode::Write); 10 | ros::Time now = ros::Time::now(); 11 | 12 | for (TrajectoryMap::const_iterator it = map_.begin(); it != map_.end(); ++it) 13 | { 14 | bag.write(it->first, now, it->second); 15 | } 16 | } 17 | 18 | void godel_surface_detection::TrajectoryLibrary::load(const std::string& filename) 19 | { 20 | rosbag::Bag bag; 21 | bag.open(filename, rosbag::bagmode::Read); 22 | rosbag::View view(bag); 23 | 24 | for (rosbag::View::iterator it = view.begin(); it != view.end(); ++it) 25 | { 26 | godel_msgs::ProcessPlanPtr ptr = it->instantiate(); 27 | if (!ptr) 28 | { 29 | // message failed to load 30 | throw std::runtime_error("Could not load joint trajectory from ROS bag"); 31 | } 32 | 33 | // Check to see if key is already in data structure 34 | std::string const& key = it->getTopic(); 35 | if (map_.find(key) != map_.end()) 36 | { 37 | throw std::runtime_error("Bagfile had name matching key already in data structure"); 38 | } 39 | 40 | map_[key] = *ptr; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /godel_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(godel_utils) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED 9 | godel_msgs) 10 | 11 | 12 | ################################### 13 | ## catkin specific configuration ## 14 | ################################### 15 | ## The catkin_package macro generates cmake config files for your package 16 | ## Declare things to be passed to dependent projects 17 | ## INCLUDE_DIRS: uncomment this if you package contains header files 18 | ## LIBRARIES: libraries you create in this project that dependent projects also need 19 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 20 | ## DEPENDS: system dependencies of this project that dependent projects also need 21 | 22 | catkin_package( 23 | INCLUDE_DIRS 24 | include 25 | LIBRARIES 26 | ${PROJECT_NAME} 27 | CATKIN_DEPENDS 28 | roscpp 29 | godel_msgs 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | include_directories( 37 | include 38 | ${catkin_INCLUDE_DIRS} 39 | ) 40 | 41 | ## Declare a C++ library 42 | add_library(${PROJECT_NAME} 43 | src/ensenso_guard.cpp 44 | ) 45 | 46 | install(TARGETS ${PROJECT_NAME} 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 50 | ) 51 | -------------------------------------------------------------------------------- /godel_utils/include/godel_utils/ensenso_guard.h: -------------------------------------------------------------------------------- 1 | #ifndef ENSENSO_GUARD_H 2 | #define ENSESNO_GUARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace ensenso 8 | { 9 | enum Command {STOP=0, START=1}; 10 | 11 | class EnsensoGuard 12 | { 13 | private: 14 | ros::NodeHandle nh_; 15 | ros::ServiceClient client_; 16 | public: 17 | EnsensoGuard(); 18 | ~EnsensoGuard(); 19 | void issueCommand(Command command); 20 | }; 21 | 22 | } 23 | 24 | #endif // ENSENSO_GUARD_H 25 | -------------------------------------------------------------------------------- /godel_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | godel_utils 4 | 0.0.1 5 | A utility package for common Godel resources 6 | 7 | ros-industrial 8 | 9 | 10 | 11 | 12 | 13 | Apache 2.0 14 | 15 | 16 | Aaron Wood 17 | 18 | catkin 19 | roscpp 20 | godel_msgs 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /godel_utils/src/ensenso_guard.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace ensenso 5 | { 6 | const static std::string SERVICE_NAME = "ensenso_manager"; 7 | 8 | EnsensoGuard::EnsensoGuard() : client_(nh_.serviceClient(SERVICE_NAME)) 9 | { 10 | issueCommand(Command::STOP); 11 | } 12 | 13 | EnsensoGuard::~EnsensoGuard() 14 | { 15 | issueCommand(Command::START); 16 | } 17 | 18 | void EnsensoGuard::issueCommand(Command command) 19 | { 20 | godel_msgs::EnsensoCommand msg; 21 | msg.request.action = command; 22 | try 23 | { 24 | client_.call(msg); 25 | } 26 | catch (const std::exception& e) 27 | { 28 | std::string command_type; 29 | switch(command) 30 | { 31 | case Command::STOP : 32 | { 33 | command_type = "stop"; 34 | } 35 | case Command::START : 36 | { 37 | command_type = "start"; 38 | } 39 | } 40 | 41 | ROS_ERROR_STREAM("Exception while attempting to " << command_type << " ensenso: " << e.what()); 42 | } 43 | } 44 | 45 | } 46 | -------------------------------------------------------------------------------- /industrial_robot_simulator_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(industrial_robot_simulator_service) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | actionlib 6 | control_msgs 7 | message_generation 8 | roscpp 9 | trajectory_msgs 10 | ) 11 | 12 | add_service_files( 13 | FILES 14 | SimulateTrajectory.srv 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | control_msgs 20 | trajectory_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | control_msgs 26 | message_runtime 27 | roscpp 28 | trajectory_msgs 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | include_directories( 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | add_executable(simulator_service_node src/simulator_service_node.cpp) 40 | 41 | add_dependencies(simulator_service_node industrial_robot_simulator_service_generate_messages_cpp) 42 | 43 | target_link_libraries(simulator_service_node 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | ############# 48 | ## Install ## 49 | ############# 50 | 51 | install(TARGETS simulator_service_node 52 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 55 | ) 56 | 57 | install( DIRECTORY launch 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | -------------------------------------------------------------------------------- /industrial_robot_simulator_service/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | industrial_robot_simulator_service 4 | 0.1.0 5 | 6 | This package adds a 'simulated' robot in its own TF namespace. Backed by ROS-I's 7 | industrial_robot_simulator, and is interacted with through a series of simple 8 | service calls. 9 | 10 | 11 | Jonathan Meyer 12 | Jonathan Meyer 13 | Apache 2.0 14 | 15 | catkin 16 | 17 | actionlib 18 | control_msgs 19 | industrial_robot_simulator 20 | roscpp 21 | trajectory_msgs 22 | 23 | message_generation 24 | 25 | message_runtime 26 | 27 | 28 | -------------------------------------------------------------------------------- /industrial_robot_simulator_service/srv/SimulateTrajectory.srv: -------------------------------------------------------------------------------- 1 | # The trajectory to execute 2 | trajectory_msgs/JointTrajectory trajectory 3 | 4 | # Set this to true if the service should block until the execution is complete 5 | bool wait_for_execution 6 | --- 7 | # Empty 8 | -------------------------------------------------------------------------------- /meshing_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(meshing_plugins) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | godel_msgs 7 | meshing_plugins_base 8 | pcl_ros 9 | pluginlib 10 | roscpp 11 | ) 12 | 13 | set(meshing_plugins_SRCS 14 | src/concave_hull_mesher.cpp 15 | ) 16 | 17 | set(meshing_plugins_HDRS 18 | include/meshing_plugins/concave_hull_plugins.h 19 | ) 20 | 21 | set(meshing_plugins_INCLUDE_DIRECTORIES 22 | include 23 | ${CMAKE_CURRENT_BINARY_DIR} 24 | ) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS 28 | ${meshing_plugins_INCLUDE_DIRECTORIES} 29 | LIBRARIES 30 | ${PROJECT_NAME} 31 | CATKIN_DEPENDS 32 | godel_msgs 33 | meshing_plugins_base 34 | pcl_ros 35 | roscpp 36 | ) 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | 42 | include_directories(${meshing_plugins_INCLUDE_DIRECTORIES} 43 | ${catkin_INCLUDE_DIRS} 44 | ${PCL_INCLUDE_DIRS} 45 | ) 46 | 47 | ## Declare a cpp library 48 | add_library(${PROJECT_NAME} 49 | ${meshing_plugins_SRCS} 50 | ${meshing_plugins_HDRS} 51 | ) 52 | 53 | target_link_libraries(${PROJECT_NAME} 54 | ${catkin_LIBRARIES} 55 | ) 56 | 57 | add_dependencies(${PROJECT_NAME} godel_msgs_generate_messages_cpp) 58 | 59 | find_package(class_loader) 60 | class_loader_hide_library_symbols(${PROJECT_NAME}) 61 | 62 | ############# 63 | ## Install ## 64 | ############# 65 | install(TARGETS ${PROJECT_NAME} 66 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 69 | 70 | install(DIRECTORY include/${PROJECT_NAME}/ 71 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 72 | ) 73 | -------------------------------------------------------------------------------- /meshing_plugins/config/meshing.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/godel/b6df38915e433ffb33065129e87e4ca7de4ef6c2/meshing_plugins/config/meshing.yaml -------------------------------------------------------------------------------- /meshing_plugins/include/meshing_plugins/concave_hull_plugins.h: -------------------------------------------------------------------------------- 1 | #ifndef CONCAVE_HULL_PLUGINS_H 2 | #define CONCAVE_HULL_PLUGINS_H 3 | 4 | #include 5 | 6 | namespace concave_hull_mesher 7 | { 8 | class ConcaveHullMesher : public meshing_plugins_base::MeshingBase 9 | { 10 | private: 11 | pcl::PointCloud input_cloud_; 12 | 13 | public: 14 | ConcaveHullMesher(){} 15 | void init(pcl::PointCloud input); 16 | bool generateMesh(pcl::PolygonMesh& mesh); 17 | }; 18 | } 19 | 20 | #endif // CONCAVE_HULL_PLUGINS_H 21 | -------------------------------------------------------------------------------- /meshing_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | meshing_plugins 4 | 0.1.0 5 | 6 | Plugins which convert point clouds to meshes 7 | 8 | 9 | Aaron Wood 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | godel_msgs 15 | meshing_plugins_base 16 | path_planning_plugins_base 17 | pcl_ros 18 | pluginlib 19 | roscpp 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /meshing_plugins/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The default meshing algorithm. Only works on 2D surfaces 5 | 6 | 7 | -------------------------------------------------------------------------------- /meshing_plugins/src/concave_hull_mesher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | const static double CONCAVE_HULL_ALPHA = 0.1; 8 | 9 | namespace concave_hull_mesher 10 | { 11 | typedef pcl::PointXYZRGB Point; 12 | typedef pcl::PointCloud PointCloud; 13 | 14 | void ConcaveHullMesher::init(PointCloud input) 15 | { 16 | input_cloud_ = input; 17 | } 18 | 19 | bool ConcaveHullMesher::generateMesh(pcl::PolygonMesh& mesh) 20 | { 21 | pcl::ConcaveHull concave_hull; 22 | pcl::EarClipping ear_clipping; 23 | pcl::PolygonMesh::Ptr mesh_ptr (new pcl::PolygonMesh); 24 | 25 | concave_hull.setInputCloud(input_cloud_.makeShared()); 26 | concave_hull.setAlpha(CONCAVE_HULL_ALPHA); 27 | concave_hull.reconstruct(*mesh_ptr); 28 | 29 | ear_clipping.setInputMesh(mesh_ptr); 30 | ear_clipping.process(mesh); 31 | 32 | return mesh.polygons.size() > 0; 33 | } 34 | } // end mesher_plugins 35 | 36 | PLUGINLIB_EXPORT_CLASS(concave_hull_mesher::ConcaveHullMesher, meshing_plugins_base::MeshingBase) 37 | -------------------------------------------------------------------------------- /meshing_plugins_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(meshing_plugins_base) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | find_package(catkin REQUIRED COMPONENTS 8 | pcl_ros 9 | roscpp 10 | ) 11 | 12 | set(meshing_plugins_HDRS 13 | include/meshing_plugins_base/meshing_base.h 14 | ) 15 | 16 | set(meshing_plugins_INCLUDE_DIRECTORIES 17 | include 18 | ${CMAKE_CURRENT_BINARY_DIR} 19 | ) 20 | 21 | catkin_package( 22 | INCLUDE_DIRS 23 | ${meshing_plugins_INCLUDE_DIRECTORIES} 24 | CATKIN_DEPENDS 25 | pcl_ros 26 | roscpp 27 | ) 28 | 29 | ########### 30 | ## Build ## 31 | ########### 32 | 33 | include_directories(${meshing_plugins_INCLUDE_DIRECTORIES} 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | 38 | ############# 39 | ## Install ## 40 | ############# 41 | 42 | install( 43 | DIRECTORY include/${PROJECT_NAME}/ 44 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 45 | -------------------------------------------------------------------------------- /meshing_plugins_base/include/meshing_plugins_base/meshing_base.h: -------------------------------------------------------------------------------- 1 | #ifndef MESHING_PLUGINS_BASE_H_ 2 | #define MESHING_PLUGINS_BASE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace meshing_plugins_base 9 | { 10 | class MeshingBase 11 | { 12 | public: 13 | /** 14 | * @brief Initialize the mesher 15 | * @param pcl::PointCloud input cloud for the mesher 16 | */ 17 | virtual void init(pcl::PointCloud input) = 0; 18 | 19 | /** 20 | * @brief Generates a pcl::PolygonMesh from the input PointCloud 21 | * @param mesh: destination variable for the resulting mesh 22 | * @return true if the generation was successful, false if it failed 23 | */ 24 | virtual bool generateMesh(pcl::PolygonMesh& mesh) = 0; 25 | }; 26 | } // end namespace meshing_plugins_base 27 | 28 | #endif // end MESHING_PLUGINS_BASE_H_ 29 | -------------------------------------------------------------------------------- /meshing_plugins_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | meshing_plugins_base 4 | 0.1.0 5 | 6 | Base class for plugins which convert pcl::PointCloud to pcl::PolygonMesh 7 | 8 | 9 | Aaron Wood 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | pcl_ros 15 | roscpp 16 | 17 | 18 | -------------------------------------------------------------------------------- /path_planning_plugins/config/path_planning.yaml: -------------------------------------------------------------------------------- 1 | path_planning_params: 2 | discretization: 0.0025 3 | margin: 0.001 4 | overlap: 0.0 5 | safe_traverse_height: 0.05 6 | scan_width: 0.01 7 | tool_radius: 0.0125 8 | -------------------------------------------------------------------------------- /path_planning_plugins/include/profilometer/profilometer_scan.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This defines some utilities for generating keyence scan paths from a 3 | * polygon boundary. 4 | * 5 | * The input is a PolygonBoundary which defines the 2d points comprising 6 | * a hole-less surface 7 | * 8 | * The output is a set of dense points which, when packaged with the associated 9 | * pose of the surface, are suitable for trajectory planning. 10 | */ 11 | #ifndef PROFILOMETER_SCAN_H 12 | #define PROFILOMETER_SCAN_H 13 | 14 | #include 15 | #include 16 | namespace path_planning_plugins 17 | { 18 | namespace scan 19 | { 20 | 21 | std::vector 22 | generateProfilometerScanPath(const godel_process_path::PolygonBoundary& boundary, 23 | const godel_msgs::PathPlanningParameters& params); 24 | 25 | } // end namespace scan 26 | } // end namespace path_planning_plugins 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /path_planning_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_planning_plugins 4 | 0.0.1 5 | 6 | Plugins which plan paths over a mesh 7 | 8 | 9 | Aaron Wood 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | cmake_modules 15 | eigen_conversions 16 | geometry_msgs 17 | godel_msgs 18 | godel_process_path_generation 19 | path_planning_plugins_base 20 | pcl_ros 21 | pluginlib 22 | roscpp 23 | tf 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /path_planning_plugins/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | Path planning plugin based on Openveronoi which plans for blend paths 6 | 7 | 9 | Path planning plugin based on Openveronoi which plans for scan paths 10 | 11 | 12 | -------------------------------------------------------------------------------- /path_planning_plugins_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(path_planning_plugins_base) 3 | 4 | add_compile_options( -std=c++11 ) 5 | 6 | ## Find catkin macros and libraries 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | pcl_ros 10 | roscpp 11 | ) 12 | 13 | set(path_planning_plugins_HDRS 14 | include/path_planning_plugins_base/path_planning_base.h 15 | ) 16 | 17 | set(path_planning_plugins_INCLUDE_DIRECTORIES 18 | include 19 | ${CMAKE_CURRENT_BINARY_DIR} 20 | ) 21 | 22 | catkin_package( 23 | INCLUDE_DIRS 24 | ${path_planning_plugins_INCLUDE_DIRECTORIES} 25 | CATKIN_DEPENDS 26 | geometry_msgs 27 | pcl_ros 28 | roscpp 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | include_directories( 36 | ${path_planning_plugins_INCLUDE_DIRECTORIES} 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | 41 | ############# 42 | ## Install ## 43 | ############# 44 | 45 | install( 46 | DIRECTORY include/${PROJECT_NAME}/ 47 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 48 | ) 49 | -------------------------------------------------------------------------------- /path_planning_plugins_base/include/path_planning_plugins_base/path_planning_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_PLANNING_PLUGINS_BASE_H_ 2 | #define PATH_PLANNING_PLUGINS_BASE_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace path_planning_plugins_base 8 | { 9 | class PathPlanningBase 10 | { 11 | public: 12 | virtual void init(pcl::PolygonMesh mesh) = 0; 13 | virtual bool generatePath(std::vector& path) = 0; 14 | }; 15 | } 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /path_planning_plugins_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_planning_plugins_base 4 | 0.0.1 5 | 6 | Base class for meshing plugins 7 | 8 | 9 | Aaron Wood 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | geometry_msgs 15 | pcl_ros 16 | roscpp 17 | 18 | 19 | --------------------------------------------------------------------------------