diff --git a/.gitmodules b/.gitmodules index 90603125..4553cfab 100644 --- a/.gitmodules +++ b/.gitmodules @@ -43,7 +43,4 @@ path = submodules/match_robot_controllers url = https://github.com/match-ROS/match_robot_controllers.git branch = main -[submodule "submodules/gazebo_ros_link_attacher"] - path = submodules/gazebo_ros_link_attacher - url = https://github.com/pal-robotics/gazebo_ros_link_attacher - branch = melodic-devel + diff --git a/general_mur_gui/CMakeLists.txt b/general_mur_gui/CMakeLists.txt new file mode 100644 index 00000000..06283b2b --- /dev/null +++ b/general_mur_gui/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(general_mur_gui) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES general_mur_gui +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/general_mur_gui.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/general_mur_gui_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_general_mur_gui.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/general_mur_gui/config/gui_persistence.yaml b/general_mur_gui/config/gui_persistence.yaml new file mode 100644 index 00000000..b373a9c0 --- /dev/null +++ b/general_mur_gui/config/gui_persistence.yaml @@ -0,0 +1,119 @@ +path_index: + updated_at: '2025-12-22 16:44:56' + value: 1200 +robots: + mur620b: + driver_nodes: + - /dummy_lift_mur620b_UR10_l + - /dummy_lift_mur620b_UR10_r + - /mur620b/UR10_r/UR10_r_enable + - /mur620b/UR10_r/controller_stopper + - /mur620b/UR10_r/gripper_interface + - /mur620b/UR10_r/pose_composer_rosmaster_40191_1833764174809868928 + - /mur620b/UR10_r/pose_composer_rosmaster_40191_3353493515075907955 + - /mur620b/UR10_r/ros_control_controller_spawner + - /mur620b/UR10_r/ros_control_stopped_spawner + - /mur620b/UR10_r/rosnode_killer + - /mur620b/UR10_r/ur_calibrated_pose_pub_node + - /mur620b/UR10_r/ur_hardware_interface + - /mur620b/UR10_r/ur_hardware_interface/ur_robot_state_helper + - /mur620b/UR10_r/ur_twist_limiter + - /mur620b/b_rep117_laser_filter + - /mur620b/bms_can_node + - /mur620b/external_localization_broadcaster + - /mur620b/f_rep117_laser_filter + - /mur620b/fake_mir_joint_publisher + - /mur620b/light_cmd_transcode + - /mur620b/light_cmd_transcode_local + - /mur620b/mir_battery_state_publisher + - /mur620b/mir_bridge + - /mur620b/mir_pose_simple + - /mur620b/move_base_node + - /mur620b/move_group + - /mur620b/odom_transformer + - /mur620b/remove_tf_frames + - /mur620b/rgb_control + - /mur620b/robot_state_publisher + - /mur620b/tf_remove_state_publisher_frames + updated_at: '2025-12-22 16:21:53' + mur620c: + driver_nodes: + - /mur620c/UR10_r/UR10_r_enable + - /mur620c/UR10_r/controller_stopper + - /mur620c/UR10_r/ewellix_tlt_node_r + - /mur620c/UR10_r/gripper_interface + - /mur620c/UR10_r/pose_composer_rosmaster_54388_6894020522162214424 + - /mur620c/UR10_r/pose_composer_rosmaster_54388_7415755212050513050 + - /mur620c/UR10_r/ros_control_controller_spawner + - /mur620c/UR10_r/ros_control_stopped_spawner + - /mur620c/UR10_r/rosnode_killer + - /mur620c/UR10_r/ur_calibrated_pose_pub_node + - /mur620c/UR10_r/ur_hardware_interface + - /mur620c/UR10_r/ur_hardware_interface/ur_robot_state_helper + - /mur620c/UR10_r/ur_twist_limiter + - /mur620c/b_rep117_laser_filter + - /mur620c/bms_can_node + - /mur620c/external_localization_broadcaster + - /mur620c/f_rep117_laser_filter + - /mur620c/fake_mir_joint_publisher + - /mur620c/joint_state_aggregator + - /mur620c/light_cmd_transcode + - /mur620c/light_cmd_transcode_local + - /mur620c/mir_battery_state_publisher + - /mur620c/mir_bridge + - /mur620c/mir_pose_simple + - /mur620c/move_base_node + - /mur620c/move_group + - /mur620c/odom_transformer + - /mur620c/remove_tf_frames + - /mur620c/rgb_control + - /mur620c/robot_state_publisher + - /mur620c/tf_remove_state_publisher_frames + spray_distance: + updated_at: '2025-12-22 16:24:34' + value: 0.3 + updated_at: '2025-12-22 16:21:53' + mur620d: + driver_nodes: + - /mur620d/UR10_r/UR10_r_enable + - /mur620d/UR10_r/controller_stopper + - /mur620d/UR10_r/ewellix_tlt_node_r + - /mur620d/UR10_r/gripper_interface + - /mur620d/UR10_r/pose_composer_rosmaster_51674_1544232900518303808 + - /mur620d/UR10_r/pose_composer_rosmaster_51674_7135075766973754761 + - /mur620d/UR10_r/ros_control_controller_spawner + - /mur620d/UR10_r/ros_control_stopped_spawner + - /mur620d/UR10_r/rosnode_killer + - /mur620d/UR10_r/ur_calibrated_pose_pub_node + - /mur620d/UR10_r/ur_hardware_interface + - /mur620d/UR10_r/ur_hardware_interface/ur_robot_state_helper + - /mur620d/UR10_r/ur_twist_limiter + - /mur620d/b_rep117_laser_filter + - /mur620d/bms_can_node + - /mur620d/external_localization_broadcaster + - /mur620d/f_rep117_laser_filter + - /mur620d/fake_mir_joint_publisher + - /mur620d/joint_state_aggregator + - /mur620d/light_cmd_transcode + - /mur620d/light_cmd_transcode_local + - /mur620d/mir_battery_state_publisher + - /mur620d/mir_bridge + - /mur620d/mir_pose_simple + - /mur620d/move_base_node + - /mur620d/move_group + - /mur620d/odom_transformer + - /mur620d/remove_tf_frames + - /mur620d/rgb_control + - /mur620d/robot_state_publisher + - /mur620d/tf_remove_state_publisher_frames + spray_distance: + updated_at: '2025-12-22 16:26:12' + value: 0.9 + updated_at: '2025-12-22 16:21:53' +servo_targets: + left: 20.0 + right: 100.0 + updated_at: '2025-12-12 16:47:58' +spray_distance: + updated_at: '2025-12-22 16:26:12' + value: 0.9 diff --git a/general_mur_gui/config/rosconsole_debug.config b/general_mur_gui/config/rosconsole_debug.config new file mode 100644 index 00000000..0b9d547d --- /dev/null +++ b/general_mur_gui/config/rosconsole_debug.config @@ -0,0 +1,5 @@ +# Forces rosconsole to emit DEBUG logs for all ROS loggers. +# See http://wiki.ros.org/rosconsole for syntax details. +log4j.logger.ros=DEBUG +log4j.logger.ros.roscpp=DEBUG +log4j.logger.ros.rospy=DEBUG diff --git a/general_mur_gui/config/servo_calibration_defaults.json b/general_mur_gui/config/servo_calibration_defaults.json new file mode 100644 index 00000000..a9582634 --- /dev/null +++ b/general_mur_gui/config/servo_calibration_defaults.json @@ -0,0 +1,12 @@ +{ + "left": { + "max": 3326, + "min": 2769, + "zero": 1126 + }, + "right": { + "max": 263, + "min": 820, + "zero": 2463 + } +} \ No newline at end of file diff --git a/general_mur_gui/img/Logo.jpg b/general_mur_gui/img/Logo.jpg new file mode 100644 index 00000000..6edc16c8 Binary files /dev/null and b/general_mur_gui/img/Logo.jpg differ diff --git a/general_mur_gui/img/Logo.png b/general_mur_gui/img/Logo.png new file mode 100644 index 00000000..f0696bf0 Binary files /dev/null and b/general_mur_gui/img/Logo.png differ diff --git a/general_mur_gui/img/Logo_small.png b/general_mur_gui/img/Logo_small.png new file mode 100644 index 00000000..ece0abcf Binary files /dev/null and b/general_mur_gui/img/Logo_small.png differ diff --git a/general_mur_gui/launch/enable_all_URs.launch b/general_mur_gui/launch/enable_all_URs.launch new file mode 100644 index 00000000..7d51d549 --- /dev/null +++ b/general_mur_gui/launch/enable_all_URs.launch @@ -0,0 +1,10 @@ + + + + + + + $(arg robot_names) + $(arg UR_prefixes) + + \ No newline at end of file diff --git a/general_mur_gui/launch/gui.launch b/general_mur_gui/launch/gui.launch new file mode 100644 index 00000000..40d3b99f --- /dev/null +++ b/general_mur_gui/launch/gui.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/general_mur_gui/launch/increment_path_index.launch b/general_mur_gui/launch/increment_path_index.launch new file mode 100644 index 00000000..8608d757 --- /dev/null +++ b/general_mur_gui/launch/increment_path_index.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/general_mur_gui/launch/launch_rviz.launch b/general_mur_gui/launch/launch_rviz.launch new file mode 100644 index 00000000..1fe69887 --- /dev/null +++ b/general_mur_gui/launch/launch_rviz.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/general_mur_gui/launch/turn_on_all_arm_controllers.launch b/general_mur_gui/launch/turn_on_all_arm_controllers.launch new file mode 100644 index 00000000..75b1910f --- /dev/null +++ b/general_mur_gui/launch/turn_on_all_arm_controllers.launch @@ -0,0 +1,10 @@ + + + + + + + $(arg robot_names) + $(arg UR_prefixes) + + \ No newline at end of file diff --git a/general_mur_gui/launch/turn_on_all_twist_controllers.launch b/general_mur_gui/launch/turn_on_all_twist_controllers.launch new file mode 100644 index 00000000..bd8ac9af --- /dev/null +++ b/general_mur_gui/launch/turn_on_all_twist_controllers.launch @@ -0,0 +1,10 @@ + + + + + + + $(arg robot_names) + $(arg UR_prefixes) + + \ No newline at end of file diff --git a/general_mur_gui/package.xml b/general_mur_gui/package.xml new file mode 100644 index 00000000..eb85ec66 --- /dev/null +++ b/general_mur_gui/package.xml @@ -0,0 +1,59 @@ + + + general_mur_gui + 0.0.0 + The general_mur_gui package + + + + + rosmatch + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/general_mur_gui/rviz/print_HW.rviz b/general_mur_gui/rviz/print_HW.rviz new file mode 100644 index 00000000..33ca60c4 --- /dev/null +++ b/general_mur_gui/rviz/print_HW.rviz @@ -0,0 +1,1513 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + - /PointCloud21 + - /PointStamped1 + - /Pose1 + - /MotionPlanning1 + - /Pose2 + Splitter Ratio: 0.5005428791046143 + Tree Height: 2284 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.10000000149011612 + Head Length: 0.05000000074505806 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 136; 138; 133 + Pose Style: Arrows + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.029999999329447746 + Shaft Length: 0.05000000074505806 + Topic: /mir_path_transformed + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 165; 29; 45 + Enabled: true + Head Diameter: 0.029999999329447746 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.009999999776482582 + Shaft Length: 0.10000000149011612 + Topic: /ur_path_transformed + Unreliable: false + Value: true + - Alpha: 0.20000000298023224 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + UR10_l/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_ideal: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_ideal: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_lift_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lift_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lift_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lift_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + top_module: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: mur620c/robot_description + TF Prefix: mur620c + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PrintGoal + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /next_goal + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: MiRTarget + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /mir_target_pose + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: MiRTarget + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /mir_actual_pose + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: CalibratedEE + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /mur620c/UR10_r/ur_calibrated_pose + Unreliable: false + Value: false + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + map: + Value: false + mir_target: + Value: true + mur620c/UR10_l/base: + Value: false + mur620c/UR10_l/base_ideal: + Value: false + mur620c/UR10_l/base_link: + Value: false + mur620c/UR10_l/base_link_inertia: + Value: false + mur620c/UR10_l/flange: + Value: false + mur620c/UR10_l/gripper: + Value: false + mur620c/UR10_l/tool0: + Value: false + mur620c/UR10_l/wrist_3_link: + Value: false + mur620c/UR10_r/base: + Value: false + mur620c/UR10_r/base_ideal: + Value: false + mur620c/UR10_r/base_link: + Value: false + mur620c/UR10_r/base_link_inertia: + Value: false + mur620c/UR10_r/calibrated_ee_pose: + Value: false + mur620c/UR10_r/flange: + Value: false + mur620c/UR10_r/forearm_link: + Value: false + mur620c/UR10_r/gripper: + Value: false + mur620c/UR10_r/shoulder_link: + Value: false + mur620c/UR10_r/tool0: + Value: false + mur620c/UR10_r/upper_arm_link: + Value: false + mur620c/UR10_r/wrist_1_link: + Value: false + mur620c/UR10_r/wrist_2_link: + Value: false + mur620c/UR10_r/wrist_3_link: + Value: false + mur620c/back_laser_link: + Value: false + mur620c/base_footprint: + Value: false + mur620c/base_link: + Value: false + mur620c/bl_caster_rotation_link: + Value: false + mur620c/bl_caster_wheel_link: + Value: false + mur620c/br_caster_rotation_link: + Value: false + mur620c/br_caster_wheel_link: + Value: false + mur620c/camera_bottom_screw_frame: + Value: false + mur620c/camera_color_frame: + Value: false + mur620c/camera_color_optical_frame: + Value: false + mur620c/camera_depth_frame: + Value: false + mur620c/camera_depth_optical_frame: + Value: false + mur620c/camera_floor_left_color_frame: + Value: false + mur620c/camera_floor_left_color_optical_frame: + Value: false + mur620c/camera_floor_left_depth_optical_frame: + Value: false + mur620c/camera_floor_left_link: + Value: false + mur620c/camera_floor_right_color_frame: + Value: false + mur620c/camera_floor_right_color_optical_frame: + Value: false + mur620c/camera_floor_right_depth_optical_frame: + Value: false + mur620c/camera_floor_right_link: + Value: false + mur620c/camera_infra1_frame: + Value: false + mur620c/camera_infra1_optical_frame: + Value: false + mur620c/camera_infra2_frame: + Value: false + mur620c/camera_infra2_optical_frame: + Value: false + mur620c/camera_link: + Value: false + mur620c/fl_caster_rotation_link: + Value: false + mur620c/fl_caster_wheel_link: + Value: false + mur620c/fr_caster_rotation_link: + Value: false + mur620c/fr_caster_wheel_link: + Value: false + mur620c/front_laser_link: + Value: false + mur620c/imu_frame: + Value: false + mur620c/imu_link: + Value: false + mur620c/left_lift_bottom: + Value: false + mur620c/left_lift_top: + Value: false + mur620c/left_wheel_link: + Value: false + mur620c/odom: + Value: false + mur620c/right_lift_bottom: + Value: false + mur620c/right_lift_top: + Value: false + mur620c/right_wheel_link: + Value: false + mur620c/surface: + Value: false + mur620c/top_module: + Value: false + mur620c/us_1_frame: + Value: false + mur620c/us_2_frame: + Value: false + ur_target: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + mir_target: + {} + mur620c/odom: + mur620c/base_footprint: + mur620c/base_link: + mur620c/back_laser_link: + {} + mur620c/bl_caster_rotation_link: + mur620c/bl_caster_wheel_link: + {} + mur620c/br_caster_rotation_link: + mur620c/br_caster_wheel_link: + {} + mur620c/camera_bottom_screw_frame: + mur620c/camera_link: + mur620c/camera_color_frame: + mur620c/camera_color_optical_frame: + {} + mur620c/camera_depth_frame: + mur620c/camera_depth_optical_frame: + {} + mur620c/camera_floor_left_link: + mur620c/camera_floor_left_color_frame: + mur620c/camera_floor_left_color_optical_frame: + {} + mur620c/camera_floor_left_depth_optical_frame: + {} + mur620c/camera_infra1_frame: + mur620c/camera_infra1_optical_frame: + {} + mur620c/camera_infra2_frame: + mur620c/camera_infra2_optical_frame: + {} + mur620c/camera_floor_right_link: + mur620c/camera_floor_right_color_frame: + mur620c/camera_floor_right_color_optical_frame: + {} + mur620c/camera_floor_right_depth_optical_frame: + {} + mur620c/fl_caster_rotation_link: + mur620c/fl_caster_wheel_link: + {} + mur620c/fr_caster_rotation_link: + mur620c/fr_caster_wheel_link: + {} + mur620c/front_laser_link: + {} + mur620c/imu_link: + mur620c/imu_frame: + {} + mur620c/left_wheel_link: + {} + mur620c/right_wheel_link: + {} + mur620c/surface: + {} + mur620c/top_module: + mur620c/left_lift_bottom: + {} + mur620c/right_lift_bottom: + mur620c/right_lift_top: + mur620c/UR10_r/base_ideal: + mur620c/UR10_r/base_link: + mur620c/UR10_r/base: + {} + mur620c/UR10_r/base_link_inertia: + mur620c/UR10_r/calibrated_ee_pose: + {} + mur620c/UR10_r/shoulder_link: + mur620c/UR10_r/upper_arm_link: + mur620c/UR10_r/forearm_link: + mur620c/UR10_r/wrist_1_link: + mur620c/UR10_r/wrist_2_link: + mur620c/UR10_r/wrist_3_link: + mur620c/UR10_r/flange: + mur620c/UR10_r/tool0: + mur620c/UR10_r/gripper: + {} + mur620c/us_1_frame: + {} + mur620c/us_2_frame: + {} + ur_target: + {} + mur620c/UR10_l/wrist_3_link: + mur620c/UR10_l/flange: + mur620c/UR10_l/tool0: + mur620c/UR10_l/gripper: + {} + mur620c/left_lift_top: + mur620c/UR10_l/base_ideal: + mur620c/UR10_l/base_link: + mur620c/UR10_l/base: + {} + mur620c/UR10_l/base_link_inertia: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /profiles + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: false + History Length: 1 + Name: PointStamped + Queue Size: 10 + Radius: 0.009999999776482582 + Topic: /profile/center_point + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 0.30000001192092896 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /mur620c/UR10_r/global_tcp_pose + Unreliable: false + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + JointsTab_Use_Radians: true + Move Group Namespace: mur620c + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + UR10_l/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_ideal: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_ideal: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_lift_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lift_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lift_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lift_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + top_module: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: UR_arm_all + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /mur620c/planning_scene + Robot Description: mur620c/robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + UR10_l/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_ideal: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_l/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_l/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/base: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_ideal: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/flange: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + UR10_r/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + UR10_r/wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_lift_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lift_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lift_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lift_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + top_module: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Axes Length: 0.30000001192092896 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /mur620c/UR10_r/global_tcp_pose_mocap + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /mur620c/move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -7.584998607635498 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 102.75859832763672 + Target Frame: + X: 52.38804244995117 + Y: 32.499080657958984 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2806 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 5120 + X: 10240 + Y: 0 diff --git a/general_mur_gui/scripts/__pycache__/gui_layout.cpython-312.pyc b/general_mur_gui/scripts/__pycache__/gui_layout.cpython-312.pyc new file mode 100644 index 00000000..4240068c Binary files /dev/null and b/general_mur_gui/scripts/__pycache__/gui_layout.cpython-312.pyc differ diff --git a/general_mur_gui/scripts/__pycache__/relative_poses.cpython-312.pyc b/general_mur_gui/scripts/__pycache__/relative_poses.cpython-312.pyc new file mode 100644 index 00000000..ccdaca04 Binary files /dev/null and b/general_mur_gui/scripts/__pycache__/relative_poses.cpython-312.pyc differ diff --git a/general_mur_gui/scripts/__pycache__/ros_interface.cpython-312.pyc b/general_mur_gui/scripts/__pycache__/ros_interface.cpython-312.pyc new file mode 100644 index 00000000..f1dcb70f Binary files /dev/null and b/general_mur_gui/scripts/__pycache__/ros_interface.cpython-312.pyc differ diff --git a/general_mur_gui/scripts/enable_all_URs.py b/general_mur_gui/scripts/enable_all_URs.py new file mode 100755 index 00000000..0cca781a --- /dev/null +++ b/general_mur_gui/scripts/enable_all_URs.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 + +# uses the roslaunch API to enable all UR robots + +import rospy +import roslaunch +from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest, UnloadController, UnloadControllerRequest + + + +class EnableAllURs: + + def config(self): + self.robot_names = rospy.get_param('~robot_names', ['mur620a', 'mur620b','mur620c','mur620d']) + self.UR_prefixes = rospy.get_param('~UR_prefixes', ['UR10_l', 'UR10_r']) + self.node_name = rospy.get_param('~node_name', 'UR_enable') + self.launch_pkg = rospy.get_param('~launch_pkg', 'ur_utilities') + self.target_position_name = rospy.get_param('~target_position_name', 'handling_position') + self.move_group_names = rospy.get_param('~move_group_names', ['UR_arm_l', 'UR_arm_r']) + self.twist_controller_name = rospy.get_param('~twist_controller_name', 'twist_controller') + + def __init__(self): + self.config() + self.unload_twist_controllers() + self.start_move_UR_to_home_pose() + + def unload_twist_controllers(self): + for robot_name in self.robot_names: + for UR_prefix in self.UR_prefixes: + manager_ns = f"/{robot_name}/{UR_prefix}/controller_manager" + switch_service = manager_ns + "/switch_controller" + unload_service = manager_ns + "/unload_controller" + + switch_client = rospy.ServiceProxy(switch_service, SwitchController) + unload_client = rospy.ServiceProxy(unload_service, UnloadController) + + try: + rospy.loginfo(f"Waiting for {switch_service}") + switch_client.wait_for_service() + request = SwitchControllerRequest() + request.start_controllers = [] + request.stop_controllers = [self.twist_controller_name] + request.strictness = 1 + rospy.loginfo(f"Stopping {self.twist_controller_name} on {robot_name}/{UR_prefix}") + switch_client(request) + except (rospy.ServiceException, rospy.ROSException) as exc: + rospy.logwarn(f"Failed to stop {self.twist_controller_name} on {robot_name}/{UR_prefix}: {exc}") + + try: + rospy.loginfo(f"Waiting for {unload_service}") + unload_client.wait_for_service() + unload_request = UnloadControllerRequest() + unload_request.name = self.twist_controller_name + rospy.loginfo(f"Unloading {self.twist_controller_name} on {robot_name}/{UR_prefix}") + unload_client(unload_request) + except (rospy.ServiceException, rospy.ROSException) as exc: + rospy.logwarn(f"Failed to unload {self.twist_controller_name} on {robot_name}/{UR_prefix}: {exc}") + + + def start_move_UR_to_home_pose(self): + for robot_name in self.robot_names: + for UR_prefix in self.UR_prefixes: + topic = "/" + robot_name + "/" + UR_prefix + "/ur_hardware_interface" + rospy.loginfo("Enabling " + topic) + namespace = "/" + robot_name + "/" + UR_prefix + "/" + process = self.launch_ros_node(self.node_name, self.launch_pkg, self.node_name + '.py', namespace, '' , ur_hardware_interface_topic=topic) + # check if the node is still running + while process.is_alive() and not rospy.is_shutdown(): + rospy.sleep(1) + + # shutdown node + rospy.signal_shutdown('All UR robots enabled.') + + + def launch_ros_node(self,node_name, package_name, node_executable, namespace="/", node_args="", **params): + # get param names from kwargs + param_names = params.keys() + # set params on param server + rospy.loginfo("Setting params for node: " + namespace + node_name) + for param_name in param_names: + rospy.loginfo("Setting param: " + namespace + node_name + "/" + param_name + " to " + str(params[param_name])) + rospy.set_param(namespace + node_name + "/" + param_name, params[param_name]) + + package = package_name + executable = node_executable + name = node_name + node = roslaunch.core.Node(package=package, node_type=executable, name=name, namespace=namespace, + machine_name=None, args=node_args, output="screen") + + launch = roslaunch.scriptapi.ROSLaunch() + launch.start() + process = launch.launch(node) + return process + + + + +if __name__ == '__main__': + rospy.init_node('enable_all_URs') + EnableAllURs() + rospy.spin() + + + + + diff --git a/general_mur_gui/scripts/gui_layout.py b/general_mur_gui/scripts/gui_layout.py new file mode 100644 index 00000000..10682100 --- /dev/null +++ b/general_mur_gui/scripts/gui_layout.py @@ -0,0 +1,395 @@ +from PyQt5.QtWidgets import ( + QWidget, + QVBoxLayout, + QGridLayout, + QSlider, + QLineEdit, + QHBoxLayout, + QPushButton, + QLabel, + QTableWidget, + QCheckBox, + QTableWidgetItem, + QGroupBox, + QTabWidget, + QSpinBox, + QDoubleSpinBox, + QTextEdit, + QComboBox, + QDoubleSpinBox, + QDialogButtonBox, + QFormLayout, + QDialog, + QMessageBox, + QButtonGroup, + QRadioButton +) +from PyQt5 import QtCore +from PyQt5.QtCore import QTimer, pyqtSignal, pyqtSlot +from typing import Any, cast +from PyQt5.QtGui import QIcon +from ros_interface import start_status_update, ur_follow_trajectory, open_rviz, launch_drivers, quit_drivers, turn_on_arm_controllers, turn_on_twist_controllers, stop_mir_motion, stop_idx_advancer, stop_ur_motion, stop_all_but_drivers +from ros_interface import enable_all_urs, move_to_home_pose, parse_mir_path, parse_ur_path, move_mir_to_start_pose, move_ur_to_start_pose, mir_follow_trajectory, increment_path_index, target_broadcaster +from ros_interface import ROSInterface +import os +import math +import html +import json + + +Qt = cast(Any, QtCore.Qt) + + +class EnterSpinBox(QSpinBox): + """QSpinBox that emits returnPressed when user presses Enter.""" + returnPressed = pyqtSignal() + + def keyPressEvent(self, event): + key_return = getattr(Qt, 'Key_Return', None) + key_enter = getattr(Qt, 'Key_Enter', None) + if event.key() in [k for k in (key_return, key_enter) if k is not None]: + self.returnPressed.emit() + super().keyPressEvent(event) + + +class ROSGui(QWidget): + path_idx = pyqtSignal(int) + medians = pyqtSignal(float, float) + ros_log_signal = pyqtSignal(str, str, str) # level, node, text + + def __init__(self): + super().__init__() + + # ROS + window + self.ros_interface = ROSInterface(self) + self.setWindowTitle("General MuR GUI") + self.setWindowIcon(QIcon(os.path.join(os.path.dirname(__file__), '../img/Logo.png'))) + self.setGeometry(100, 100, 1600, 850) + main_layout = QHBoxLayout() + # Left column + left_layout = QVBoxLayout() + self.status_label = QLabel("Controller Status: Not Checked") + self.status_label.setStyleSheet("border: 1px solid black; padding: 5px;") + left_layout.addWidget(self.status_label) + # Battery + self.battery_group = QGroupBox("Battery Status"); self.battery_layout = QVBoxLayout(); self.battery_group.setLayout(self.battery_layout); left_layout.addWidget(self.battery_group); self.battery_labels = {} + # Selection + selection_group = QGroupBox("Robot and UR Selection"); selection_layout = QHBoxLayout(); robot_layout = QVBoxLayout() + self.robots = {n: QCheckBox(n) for n in ["mur620a", "mur620b", "mur620c", "mur620d"]} + for cb in self.robots.values(): robot_layout.addWidget(cb) + for name, cb in self.robots.items(): cb.stateChanged.connect(lambda _, r=name: self.ros_interface.check_and_subscribe_battery()) + for robot in self.robots.keys(): + row = QHBoxLayout(); row.addWidget(QLabel(robot)); mir_label = QLabel("MiR: –"); ur_label = QLabel("UR: –"); row.addWidget(mir_label); row.addWidget(ur_label); self.battery_labels[robot] = (mir_label, ur_label); self.battery_layout.addLayout(row) + ur_layout = QVBoxLayout(); ur_layout.addWidget(QLabel("Select URs:")); self.ur10_l = QCheckBox("UR10_l"); self.ur10_r = QCheckBox("UR10_r"); self.ur10_l.setChecked(False); self.ur10_r.setChecked(True); ur_layout.addWidget(self.ur10_l); ur_layout.addWidget(self.ur10_r) + selection_layout.addLayout(robot_layout); selection_layout.addLayout(ur_layout); selection_group.setLayout(selection_layout); left_layout.addWidget(selection_group) + + # Setup + setup_group = QGroupBox("Setup Functions"); setup_layout = QVBoxLayout(); + setup_buttons = { + "Check Status": lambda: start_status_update(self), + "Launch Drivers": lambda: launch_drivers(self), + "Prepare Driver Cleanup Channel": lambda: self.ros_interface.prime_driver_cleanup_sessions(), + "Launch Keyence Scanner": lambda: self.ros_interface.launch_keyence_scanner(), + "Launch Flow Sensor Bridge": lambda: self.ros_interface.launch_flow_sensor_bridge(), + "Start Dynamixel Driver": lambda: self.ros_interface.start_dynamixel_driver(), + "Stop Dynamixel Driver": lambda: self.ros_interface.stop_dynamixel_driver(), + "Launch Strand Center Camera": lambda: self.ros_interface.launch_strand_center_app(), + "Open RVIZ": lambda: open_rviz(self), + "Start Roscore": lambda: self.ros_interface.start_roscore(), + "Start Mocap": lambda: self.ros_interface.start_mocap(), + "Start Sync": lambda: self.ros_interface.start_sync(), + } + for text, fn in setup_buttons.items(): + b = QPushButton(text); + if text=="Launch Keyence Scanner": self.btn_keyence=b + if text=="Launch Flow Sensor Bridge": + self.btn_flow_sensor=b + b.setContextMenuPolicy(Qt.CustomContextMenu) + b.customContextMenuRequested.connect(self._handle_flow_sensor_right_click) + if text=="Launch Drivers": + self.btn_launch_drivers=b + b.setContextMenuPolicy(Qt.CustomContextMenu) + b.customContextMenuRequested.connect(self._handle_launch_drivers_right_click) + if text == "Start Roscore": self.btn_roscore = b + elif text == "Start Mocap": self.btn_mocap = b + elif text == "Start Sync": self.btn_sync = b + b.clicked.connect(lambda _, f=fn: f()); b.setStyleSheet("background-color: lightgray;"); setup_layout.addWidget(b) + + self.workspace_input = QLineEdit(); default_path = self.get_relative_workspace_path(); self.workspace_input.setText(default_path); self.workspace_input.setPlaceholderText("Enter workspace name"); setup_layout.addWidget(QLabel("Workspace Name:")); setup_layout.addWidget(self.workspace_input); setup_group.setLayout(setup_layout); left_layout.addWidget(setup_group) + main_layout.addLayout(left_layout) + # Right column + right_layout = QVBoxLayout(); controller_group = QGroupBox("Controller Functions"); controller_layout = QVBoxLayout(); controller_buttons = { + "Enable all URs": lambda: enable_all_urs(self), + "Turn on Arm Controllers": lambda: turn_on_arm_controllers(self), + "Turn on Twist Controllers": lambda: turn_on_twist_controllers(self), + "Move to Home Pose Left": lambda: move_to_home_pose(self, "UR10_l"), + "Move to Home Pose Right": lambda: move_to_home_pose(self, "UR10_r"), + } + for text, fn in controller_buttons.items(): btn = QPushButton(text); btn.clicked.connect(lambda _, f=fn: f()); controller_layout.addWidget(btn) + controller_group.setLayout(controller_layout); right_layout.addWidget(controller_group) + + # --- ROS log console on the far right --- + log_group = QGroupBox("ROS Messages") + log_layout = QVBoxLayout() + + self.ros_log_text = QTextEdit() + self.ros_log_text.setReadOnly(True) + self.ros_log_text.setLineWrapMode(QTextEdit.NoWrap) + + log_layout.addWidget(self.ros_log_text) + + # --- Log level filter checkboxes --- + filter_layout = QHBoxLayout() + self.chk_log_error = QCheckBox("Error") + self.chk_log_warn = QCheckBox("Warning") + self.chk_log_info = QCheckBox("Info") + self.chk_log_debug = QCheckBox("Debug") + + # Default: Error/Warn/Info an, Debug aus + self.chk_log_error.setChecked(True) + self.chk_log_warn.setChecked(True) + self.chk_log_info.setChecked(True) + self.chk_log_debug.setChecked(False) + + # Clear-Button + self.btn_log_clear = QPushButton("Clear") + self.btn_log_clear.clicked.connect(self._clear_ros_log) + + # Filter-Checkboxen triggern Ansicht neu + for cb in (self.chk_log_error, self.chk_log_warn, + self.chk_log_info, self.chk_log_debug): + cb.stateChanged.connect(self._rebuild_ros_log_view) + + filter_layout.addWidget(self.chk_log_error) + filter_layout.addWidget(self.chk_log_warn) + filter_layout.addWidget(self.chk_log_info) + filter_layout.addWidget(self.chk_log_debug) + filter_layout.addWidget(self.btn_log_clear) + + log_layout.addLayout(filter_layout) + log_group.setLayout(log_layout) + main_layout.addWidget(log_group) + + self.setLayout(main_layout) + + # connect ROS log signal after widgets exist + self._ros_log_buffer = [] + self._ros_log_update_timer = QTimer(self) + self._ros_log_update_timer.setSingleShot(True) + self._ros_log_update_timer.setInterval(120) + self._ros_log_update_timer.timeout.connect(self._flush_ros_log_view) + self.ros_log_signal.connect(self._append_ros_log) + + + self.setLayout(main_layout) + # Timer + self.status_timer = QTimer(); self.status_timer.timeout.connect(self.ros_interface.update_button_status); self.status_timer.start(2000) + + def closeEvent(self, event): + """Stop periodic updates and tear down ROS before the app quits.""" + try: + if hasattr(self, "status_timer"): + self.status_timer.stop() + except Exception as exc: + print(f"Failed to stop status timer: {exc}") + try: + self.ros_interface.shutdown() + except Exception as exc: + print(f"Failed to shut down ROS interface: {exc}") + super().closeEvent(event) + + def _handle_launch_drivers_right_click(self, _pos): + """Stop driver terminals when the launch button is right-clicked.""" + quit_drivers(self) + + def _handle_flow_sensor_right_click(self, _pos): + """Stop the flow sensor bridge when its button is right-clicked.""" + self.ros_interface.stop_flow_sensor_bridge() + + def _handle_start_signal_button(self): + """Trigger the latched start condition via the ROS interface.""" + if hasattr(self, "ros_interface"): + self.ros_interface.trigger_start_signal() + + def _publish_current_index(self): + """Send the currently selected index back onto /path_index.""" + if not hasattr(self, "idx_spin"): + return + self.ros_interface.publish_path_index(self.idx_spin.value()) + + def update_start_signal_visual(self, active: bool): + button = getattr(self, "btn_start_signal", None) + if button is None: + return + if active: + button.setText("Start Signal ACTIVE (click to retrigger)") + button.setStyleSheet("background-color: #2e7d32; color: white; font-weight: bold;") + else: + button.setText("Trigger Start Signal") + button.setStyleSheet("background-color: #4caf50; color: white;") + + + def _ros_log_level_enabled(self, level: str) -> bool: + level = level.upper() + if level == "ERROR": + return self.chk_log_error.isChecked() + if level in ("WARN", "WARNING"): + return self.chk_log_warn.isChecked() + if level == "INFO": + return self.chk_log_info.isChecked() + if level == "DEBUG": + return self.chk_log_debug.isChecked() + # Unbekannt – behandel wie INFO + return self.chk_log_info.isChecked() + + + @pyqtSlot(str, str, str) + def _append_ros_log(self, level: str, node: str, text: str): + """Append one ROS log entry to the buffer and refresh view.""" + if not hasattr(self, "_ros_log_buffer"): + self._ros_log_buffer = [] + + # Buffer enthält Tuples + self._ros_log_buffer.append((level, node, text)) + self._ros_log_buffer = self._ros_log_buffer[-400:] + + self._schedule_ros_log_refresh() + + + def _schedule_ros_log_refresh(self): + timer = getattr(self, "_ros_log_update_timer", None) + if timer is None: + self._rebuild_ros_log_view() + return + timer.start() + + + def _flush_ros_log_view(self): + timer = getattr(self, "_ros_log_update_timer", None) + if timer is not None and timer.isActive(): + timer.stop() + self._rebuild_ros_log_view() + + + def _rebuild_ros_log_view(self): + """Rebuild the log text widget from the buffer, applying filters + colors.""" + html_lines = [] + + for level, node, text in self._ros_log_buffer: + if not self._ros_log_level_enabled(level): + continue + + line = f"[{level}] {node}: {text}" + escaped = html.escape(line) + + lvl = level.upper() + if lvl in ("WARN", "WARNING"): + escaped = escaped.replace( + "[WARN]", + '[WARN]' + ) + elif lvl == "ERROR": + escaped = escaped.replace( + "[ERROR]", + '[ERROR]' + ) + + html_lines.append(escaped) + + hsb = self.ros_log_text.horizontalScrollBar() + prev_h_value = hsb.value() if hsb is not None else 0 + h_was_at_end = bool(hsb and prev_h_value >= hsb.maximum()) + + self.ros_log_text.setHtml("
".join(html_lines)) + + if hsb is not None: + if h_was_at_end: + hsb.setValue(hsb.maximum()) + else: + hsb.setValue(min(prev_h_value, hsb.maximum())) + + sb = self.ros_log_text.verticalScrollBar() + if sb is not None: + sb.setValue(sb.maximum()) + + + def _clear_ros_log(self): + """Clear all buffered log messages and the view.""" + self._ros_log_buffer = [] + self.ros_log_text.clear() + if hasattr(self, "_ros_log_update_timer"): + self._ros_log_update_timer.stop() + + def open_ur_settings(self): + dlg = URFollowSettingsDialog(self, initial_settings=self.ur_follow_settings) + if dlg.exec_() == QDialog.Accepted: + # Retrieve and store new settings + self.ur_follow_settings = dlg.getValues() + print("New UR Follow settings:", self.ur_follow_settings) + + def set_idx_metric(self, text): + self.idx_metric = text + + def get_full_workspace_path(self): + import os + current_path = os.path.abspath(__file__) + + while current_path != "/": + if os.path.basename(current_path) == "src": + return os.path.dirname(current_path) # Absoluter Pfad zum Workspace + current_path = os.path.dirname(current_path) + + return os.path.expanduser("~/catkin_ws") # Fallback + + + def get_relative_workspace_path(self): + full_path = self.get_full_workspace_path() + home_path = os.path.expanduser("~") + if full_path.startswith(home_path): + return os.path.relpath(full_path, home_path) + return full_path + + def update_virtual_object_pose(self, pose): + """Updates the GUI table with the latest virtual object pose.""" + for col in range(6): + self.table.setItem(8, col, QTableWidgetItem(str(round(pose[col], 4)))) + + def get_selected_robots(self): + return [name for name, checkbox in self.robots.items() if checkbox.isChecked()] + + def get_selected_urs(self): + ur_prefixes = [] + if self.ur10_l.isChecked(): + ur_prefixes.append("UR10_l") + if self.ur10_r.isChecked(): + ur_prefixes.append("UR10_r") + return ur_prefixes + + def get_workspace_name(self): + return self.workspace_input.text().strip() + + def get_override_value(self): + return self.override_slider.value() + + def _handle_turbo_mode_toggle(self, enabled: bool): + if not hasattr(self, 'override_slider'): + return + max_value = 200 if enabled else 100 + self.override_slider.setMaximum(max_value) + if self.override_slider.value() > max_value: + self.override_slider.setValue(max_value) + + def _handle_ludicrous_mode_toggle(self, enabled: bool): + if not hasattr(self, 'override_slider'): + return + max_value = 300 if enabled else (200 if self.is_turbo_mode_enabled() else 100) + self.override_slider.setMaximum(max_value) + if self.override_slider.value() > max_value: + self.override_slider.setValue(max_value) + + def is_debug_enabled(self): + chk = getattr(self, "chk_log_debug", None) + return bool(chk and chk.isChecked()) + + + diff --git a/general_mur_gui/scripts/increment_path_index.py b/general_mur_gui/scripts/increment_path_index.py new file mode 100755 index 00000000..7dcd17a1 --- /dev/null +++ b/general_mur_gui/scripts/increment_path_index.py @@ -0,0 +1,43 @@ +#! /usr/bin/env python3 + +import rospy +from std_msgs.msg import Int32 +def increment_path_index(): + # Initialize the ROS node + rospy.init_node('increment_path_index', anonymous=True) + + path_index_topic = rospy.get_param('~path_index_topic', '/path_index') + initial_path_index = rospy.get_param('~initial_path_index', 0) + + # Create a publisher to the 'path_index' topic + pub = rospy.Publisher(path_index_topic, Int32, queue_size=10) + + # Set the rate at which to publish messages + rate = rospy.Rate(10.0) # 3.33 Hz + + # Initialize the path index + path_index = initial_path_index + + while not rospy.is_shutdown(): + # Increment the path index + path_index += 1 + + # Create a message with the current path index + msg = Int32() + msg.data = path_index + + # Publish the message + pub.publish(msg) + + # Log the published path index + #rospy.loginfo(f"Published path index: {path_index}") + + # Sleep for the specified rate + rate.sleep() + + +if __name__ == '__main__': + try: + increment_path_index() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/general_mur_gui/scripts/main.py b/general_mur_gui/scripts/main.py new file mode 100755 index 00000000..f165a646 --- /dev/null +++ b/general_mur_gui/scripts/main.py @@ -0,0 +1,18 @@ +#! /usr/bin/env python3 +import signal +import sys +from PyQt5.QtWidgets import QApplication +from gui_layout import ROSGui + +if __name__ == "__main__": + app = QApplication(sys.argv) + window = ROSGui() + window.show() + + def _handle_sigint(_sig, _frame): + """Gracefully close the GUI when Ctrl+C is pressed in the terminal.""" + window.close() + + signal.signal(signal.SIGINT, _handle_sigint) + + sys.exit(app.exec_()) diff --git a/general_mur_gui/scripts/observation_gui.py b/general_mur_gui/scripts/observation_gui.py new file mode 100755 index 00000000..fa498025 --- /dev/null +++ b/general_mur_gui/scripts/observation_gui.py @@ -0,0 +1,397 @@ +#!/usr/bin/env python3 +"""Interactive GUI to observe UR-related twist topics.""" + +import json +import sys +import threading +from collections import deque +from pathlib import Path + +import rospy +from geometry_msgs.msg import Twist +from python_qt_binding import QtCore, QtWidgets # type: ignore[attr-defined] +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from matplotlib.figure import Figure + +# Available twist components that can be plotted +TWIST_COMPONENTS = ( + "linear.x", + "linear.y", + "linear.z", + "angular.x", + "angular.y", + "angular.z", +) + +DEFAULT_SELECTED_COMPONENTS = ("linear.x", "linear.y", "linear.z") + +# Topics that appear in complete_ur_trajectory_follower_ff_only.launch +DEFAULT_TWIST_TOPICS = [ + "/ur_twist_mir_compensation", + "/ur_twist_world_in_mir", + "/laser_profile_offset_cmd_vel", + "/orthogonal_twist", + "/ur_twist_direction_world", + "/ur_rotation_twist_world", + "/ur_twist_world", + "/ur_error_world", + "/orthogonal_error", +] + + +class TwistBuffer: + """Stores recent samples for a single twist topic.""" + + def __init__(self, max_len): + self.max_len = max_len + self.timestamps = deque(maxlen=max_len) + self.data = {component: deque(maxlen=max_len) for component in TWIST_COMPONENTS} + self.lock = threading.Lock() + + def append(self, stamp, msg): + values = { + "linear.x": msg.linear.x, + "linear.y": msg.linear.y, + "linear.z": msg.linear.z, + "angular.x": msg.angular.x, + "angular.y": msg.angular.y, + "angular.z": msg.angular.z, + } + with self.lock: + self.timestamps.append(stamp) + for component, component_values in self.data.items(): + component_values.append(values[component]) + + def get_series(self, component): + with self.lock: + if not self.timestamps: + return [], [] + times = list(self.timestamps) + base = times[0] + rel_times = [t - base for t in times] + values = list(self.data[component]) + return rel_times, values + + def clear(self): + with self.lock: + self.timestamps.clear() + for component_values in self.data.values(): + component_values.clear() + + +class ObservationGUI(QtWidgets.QWidget): + def __init__(self): + super().__init__() + rospy.init_node("observation_gui", anonymous=True) + + robot_name = str(rospy.get_param("~robot_name", "mur620d")) + prefix_ur_param = rospy.get_param("~prefix_ur", "UR10_r/") + prefix_ur = str(prefix_ur_param) + if not prefix_ur.endswith("/"): + prefix_ur += "/" + + combined_topic = f"/{robot_name}/{prefix_ur}twist_controller/command_collision_free" + + twist_topics = list(DEFAULT_TWIST_TOPICS) + twist_topics.append(combined_topic) + extra_topics = rospy.get_param("~twist_topics", []) + if isinstance(extra_topics, (list, tuple, set)): + twist_topics.extend([str(topic) for topic in extra_topics if topic]) + elif extra_topics: + twist_topics.append(str(extra_topics)) + self.twist_topics = sorted({topic for topic in twist_topics if topic}) + + self.max_points = self._coerce_to_int(rospy.get_param("~max_points", 2000), 2000) + self.update_interval_ms = self._coerce_to_int(rospy.get_param("~update_interval_ms", 200), 200) + default_config_path = Path.home() / ".ros" / "observation_gui_config.json" + config_param = rospy.get_param("~config_path", str(default_config_path)) + self.config_path = Path(str(config_param)) + + self.buffers = {topic: TwistBuffer(self.max_points) for topic in self.twist_topics} + + self._subscribers = [ + rospy.Subscriber(topic, Twist, self._make_callback(topic), queue_size=10) + for topic in self.twist_topics + ] + + self._build_ui() + self._start_update_timer() + self._load_config(initial=True) + + rospy.loginfo("observation_gui ready. Monitoring %d twist topics.", len(self.twist_topics)) + + def _make_callback(self, topic): + def _callback(msg): + self.buffers[topic].append(rospy.get_time(), msg) + + return _callback + + @staticmethod + def _coerce_to_int(value, default): + if isinstance(value, int): + return value + if isinstance(value, float): + return int(value) + try: + return int(str(value)) + except (TypeError, ValueError): + rospy.logwarn("Invalid integer parameter '%s', falling back to %d", value, default) + return default + + def _build_ui(self): + self.setWindowTitle("Observation GUI - Twist Monitor") + self.resize(1100, 700) + + main_layout = QtWidgets.QVBoxLayout(self) + + controls_layout = QtWidgets.QHBoxLayout() + main_layout.addLayout(controls_layout) + + self.topic_list = QtWidgets.QListWidget() + self.topic_list.setSelectionMode(QtWidgets.QAbstractItemView.NoSelection) + for topic in self.twist_topics: + self._insert_topic_item(topic, checked=True) + topic_group = QtWidgets.QGroupBox("Twist Topics") + topic_group_layout = QtWidgets.QVBoxLayout(topic_group) + topic_group_layout.addWidget(self.topic_list) + + topic_buttons_layout = QtWidgets.QHBoxLayout() + self.select_all_button = QtWidgets.QPushButton("Select All") + self.select_all_button.clicked.connect(lambda: self._set_all_topics(True)) + self.deselect_all_button = QtWidgets.QPushButton("Deselect All") + self.deselect_all_button.clicked.connect(lambda: self._set_all_topics(False)) + topic_buttons_layout.addWidget(self.select_all_button) + topic_buttons_layout.addWidget(self.deselect_all_button) + topic_group_layout.addLayout(topic_buttons_layout) + + add_topic_layout = QtWidgets.QHBoxLayout() + self.custom_topic_input = QtWidgets.QLineEdit() + self.custom_topic_input.setPlaceholderText("Topic name (e.g., /my_twist_topic)") + self.add_topic_button = QtWidgets.QPushButton("Add Topic") + self.add_topic_button.clicked.connect(self._handle_add_topic) + add_topic_layout.addWidget(self.custom_topic_input) + add_topic_layout.addWidget(self.add_topic_button) + topic_group_layout.addLayout(add_topic_layout) + controls_layout.addWidget(topic_group, stretch=2) + + component_group = QtWidgets.QGroupBox("Components") + component_layout = QtWidgets.QVBoxLayout(component_group) + self.component_checks = {} + for component in TWIST_COMPONENTS: + cb = QtWidgets.QCheckBox(component) + cb.setChecked(component in DEFAULT_SELECTED_COMPONENTS) + self.component_checks[component] = cb + component_layout.addWidget(cb) + controls_layout.addWidget(component_group, stretch=1) + + button_layout = QtWidgets.QVBoxLayout() + self.clear_button = QtWidgets.QPushButton("Clear Data") + self.clear_button.clicked.connect(self._clear_data) + button_layout.addWidget(self.clear_button) + + self.save_config_button = QtWidgets.QPushButton("Save Config") + self.save_config_button.clicked.connect(self._save_config) + button_layout.addWidget(self.save_config_button) + + self.load_config_button = QtWidgets.QPushButton("Load Config") + self.load_config_button.clicked.connect(lambda: self._load_config()) + button_layout.addWidget(self.load_config_button) + button_layout.addStretch() + controls_layout.addLayout(button_layout) + + self.figure = Figure(figsize=(8, 5)) + self.canvas = FigureCanvas(self.figure) + self.axes = self.figure.add_subplot(111) + self.axes.set_xlabel("Time [s]") + self.axes.set_ylabel("Twist value") + self.axes.grid(True) + main_layout.addWidget(self.canvas, stretch=1) + + def _set_all_topics(self, enabled): + state = QtCore.Qt.Checked if enabled else QtCore.Qt.Unchecked + for index in range(self.topic_list.count()): + self.topic_list.item(index).setCheckState(state) + + def _handle_add_topic(self): + topic = self.custom_topic_input.text().strip() + if not topic: + return + self._add_topic(topic, auto_select=True) + self.custom_topic_input.clear() + + def _add_topic(self, topic, auto_select): + topic = topic.strip() + if not topic: + return + if topic in self.buffers: + self._set_topic_check_state(topic, auto_select) + return + self.twist_topics.append(topic) + self.twist_topics.sort() + self.buffers[topic] = TwistBuffer(self.max_points) + self._subscribers.append( + rospy.Subscriber(topic, Twist, self._make_callback(topic), queue_size=10) + ) + self._insert_topic_item(topic, auto_select) + + def _insert_topic_item(self, topic, checked): + item = QtWidgets.QListWidgetItem(topic) + item.setFlags(item.flags() | QtCore.Qt.ItemIsUserCheckable) + item.setCheckState(QtCore.Qt.Checked if checked else QtCore.Qt.Unchecked) + insert_row = self.topic_list.count() + for index in range(self.topic_list.count()): + if self.topic_list.item(index).text() > topic: + insert_row = index + break + self.topic_list.insertItem(insert_row, item) + + def _find_topic_item(self, topic): + for index in range(self.topic_list.count()): + item = self.topic_list.item(index) + if item.text() == topic: + return item + return None + + def _set_topic_check_state(self, topic, enabled): + item = self._find_topic_item(topic) + if item is None: + return + item.setCheckState(QtCore.Qt.Checked if enabled else QtCore.Qt.Unchecked) + + def _start_update_timer(self): + self.timer = QtCore.QTimer(self) + self.timer.timeout.connect(self._update_plot) + self.timer.start(self.update_interval_ms) + + def _selected_topics(self): + result = [] + for index in range(self.topic_list.count()): + item = self.topic_list.item(index) + if item.checkState() == QtCore.Qt.Checked: + result.append(item.text()) + return result + + def _selected_components(self): + return [name for name, checkbox in self.component_checks.items() if checkbox.isChecked()] + + def _gather_config_payload(self): + topics_state = [] + for index in range(self.topic_list.count()): + item = self.topic_list.item(index) + topics_state.append( + { + "name": item.text(), + "enabled": item.checkState() == QtCore.Qt.Checked, + } + ) + components_state = { + name: checkbox.isChecked() for name, checkbox in self.component_checks.items() + } + return { + "topics": topics_state, + "components": components_state, + } + + def _save_config(self): + payload = self._gather_config_payload() + try: + self.config_path.parent.mkdir(parents=True, exist_ok=True) + with self.config_path.open("w", encoding="utf-8") as handle: + json.dump(payload, handle, indent=2) + rospy.loginfo("Saved observation_gui config to %s", self.config_path) + except OSError as exc: + rospy.logerr("Failed to save config to %s: %s", self.config_path, exc) + + def _load_config(self, initial=False): + if not self.config_path.exists(): + if not initial: + rospy.logwarn("Config file %s not found", self.config_path) + return + try: + with self.config_path.open("r", encoding="utf-8") as handle: + data = json.load(handle) + except (OSError, json.JSONDecodeError) as exc: + rospy.logerr("Failed to load config %s: %s", self.config_path, exc) + return + + topics = data.get("topics", []) + if isinstance(topics, list) and topics: + self._set_all_topics(False) + for entry in topics: + topic_name = str(entry.get("name", "")).strip() + if not topic_name: + continue + enabled = bool(entry.get("enabled", True)) + self._add_topic(topic_name, auto_select=enabled) + + components = data.get("components", {}) + if isinstance(components, dict): + for name, state in components.items(): + checkbox = self.component_checks.get(name) + if checkbox is not None: + checkbox.setChecked(bool(state)) + + rospy.loginfo("Loaded observation_gui config from %s", self.config_path) + + def _clear_data(self): + for buffer in self.buffers.values(): + buffer.clear() + self.axes.clear() + self.axes.set_xlabel("Time [s]") + self.axes.set_ylabel("Twist value") + self.axes.grid(True) + self.canvas.draw_idle() + + def _update_plot(self): + if rospy.is_shutdown(): + QtWidgets.QApplication.quit() + return + + topics = self._selected_topics() + components = self._selected_components() + + self.axes.clear() + self.axes.set_xlabel("Time [s]") + self.axes.set_ylabel("Twist value") + self.axes.grid(True) + + if not topics or not components: + self.axes.set_title("Select at least one topic and one component") + self.canvas.draw_idle() + return + + plotted_any = False + for topic in topics: + buffer = self.buffers.get(topic) + if buffer is None: + continue + for component in components: + times, values = buffer.get_series(component) + if not times: + continue + label = f"{topic} [{component}]" + self.axes.plot(times, values, label=label) + plotted_any = True + + if plotted_any: + self.axes.legend(loc="upper right", fontsize="small") + else: + self.axes.set_title("Waiting for data...") + + self.canvas.draw_idle() + + +def main(): + app = QtWidgets.QApplication(sys.argv) + gui = ObservationGUI() + gui.show() + + rospy.on_shutdown(app.quit) + sys.exit(app.exec_()) + + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/general_mur_gui/scripts/poses.yaml b/general_mur_gui/scripts/poses.yaml new file mode 100644 index 00000000..2d9414df --- /dev/null +++ b/general_mur_gui/scripts/poses.yaml @@ -0,0 +1,63 @@ +Virtual Object: +- 35.8978 +- 32.1362 +- 1.514 +- 0.0005 +- -0.0079 +- -2.1464 +mur620a/UR10_l: +- 0.19095814536964512 +- 0.24046850941222764 +- 0.0 +- 3.093209691855088 +- -0.008939200594645286 +- 1.6488044279231941 +mur620a/UR10_r: +- 0.2021301790384606 +- 1.2531271665660435 +- 0.0 +- 3.0773531986236193 +- -0.001531143692429265 +- 1.6143615039178731 +mur620b/UR10_l: +- 0.15 +- 0.0 +- -0.0010315731500602277 +- 3.1360644261751833 +- 1.57079632679 +- 0.0 +mur620b/UR10_r: +- -0.0007667901108114734 +- 0.15 +- 0.001049136656738554 +- 3.1360644261751833 +- 1.57079632679 +- 1.57079632679 +mur620c/UR10_l: +- 0.47613358151741536 +- 0.25955164081571497 +- 0.005891674402632179 +- -3.1406358817406392 +- 0.0 +- -1.605913556095107 +mur620c/UR10_r: +- -0.5085582466871197 +- 0.2551900883588602 +- 0.0008393202050600377 +- -3.141373500625495 +- -0.007036856312932229 +- 0.0 +mur620d/UR10_l: +- -0.47115791687949393 +- -0.26697308190182106 +- -0.0042080829284012735 +- 3.141544730104127 +- 0.009332763907145181 +- 1.5720883236553491 +mur620d/UR10_r: +- 0.5036097550767894 +- -0.2477591634058155 +- -0.0025432943443157094 +- -3.1415012772019395 +- 0.007703736463439411 +- 1.5728178966144157 diff --git a/general_mur_gui/scripts/ros_interface.py b/general_mur_gui/scripts/ros_interface.py new file mode 100644 index 00000000..79fac312 --- /dev/null +++ b/general_mur_gui/scripts/ros_interface.py @@ -0,0 +1,2192 @@ +import os +import subprocess +import yaml +import threading +import shlex +from typing import Optional, Iterable +import signal +import rospy +from geometry_msgs.msg import PoseStamped +from PyQt5.QtWidgets import QTableWidgetItem +from PyQt5.QtCore import QTimer +import rospy +import tf.transformations as tf_trans +from rosgraph_msgs.msg import Log +from std_msgs.msg import Float32, Int32, Int16, Bool +from sensor_msgs.msg import BatteryState +from rosgraph_msgs.msg import Log +import rosnode +import time +import rospkg + +from dynamixel_workbench_msgs.msg import DynamixelStateList + +import rospy +from geometry_msgs.msg import PoseStamped + + +DEBUG_CONFIG_PATH = os.path.abspath( + os.path.join(os.path.dirname(__file__), "..", "config", "rosconsole_debug.config") +) + +GUI_CACHE_PATH = os.path.abspath( + os.path.join(os.path.dirname(__file__), "..", "config", "gui_persistence.yaml") +) + +DEFAULT_SPRAY_DISTANCE = 0.52 +DEFAULT_PATH_INDEX = 0 +DEFAULT_COMPONENT_NAME = "rectangleRoundedCorners" +COMPONENT_TRANSFORM_KEYS = ("tx", "ty", "tz", "rx", "ry", "rz") +ROSCORE_HOST = "roscore" + + +def _deploy_gui_cache_to_robot(robot: str, workspace: Optional[str]): + """Copy the GUI cache (driver nodes, spray distance, …) onto the robot's workspace.""" + robot = (robot or "").strip() + workspace = (workspace or "").strip() + if not (robot and workspace): + return + if not os.path.exists(GUI_CACHE_PATH): + return + + remote_path = f"{robot}:~/{workspace}/src/match_additive_manufacturing/print_gui/config/gui_persistence.yaml" + try: + subprocess.check_call([ + "scp", + GUI_CACHE_PATH, + remote_path, + ]) + except subprocess.CalledProcessError as exc: + print(f"Failed to copy GUI cache to {robot}: {exc}") + + +def _is_debug_enabled(gui) -> bool: + return bool(getattr(gui, "is_debug_enabled", lambda: False)()) + + +def _build_debug_env(gui, base_env=None): + env = os.environ.copy() if base_env is None else base_env.copy() + if _is_debug_enabled(gui): + env["ROSCONSOLE_CONFIG_FILE"] = DEBUG_CONFIG_PATH + else: + env.pop("ROSCONSOLE_CONFIG_FILE", None) + return env + + +def _remote_debug_prefix(gui, workspace: Optional[str] = None) -> str: + workspace = (workspace or "").strip() + if not (_is_debug_enabled(gui) and workspace): + return "" + remote_config = f"~/{workspace}/src/match_additive_manufacturing/print_gui/config/rosconsole_debug.config" + return f"ROSCONSOLE_CONFIG_FILE={remote_config}; " + + +def _popen_with_debug(command, gui, shell=False, **kwargs): + env = _build_debug_env(gui, kwargs.pop("env", None)) + return subprocess.Popen(command, shell=shell, env=env, **kwargs) + + +def _load_gui_cache_payload() -> dict: + try: + with open(GUI_CACHE_PATH, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + except FileNotFoundError: + return {} + except (yaml.YAMLError, OSError) as exc: + print(f"Failed to read GUI cache: {exc}") + return {} + return data if isinstance(data, dict) else {} + + +def _save_gui_cache_payload(payload: dict): + os.makedirs(os.path.dirname(GUI_CACHE_PATH), exist_ok=True) + with open(GUI_CACHE_PATH, "w", encoding="utf-8") as stream: + yaml.safe_dump(payload, stream, default_flow_style=False, sort_keys=True) + + +def _load_servo_targets_cache(default_left: float = 0.0, default_right: float = 0.0): + data = _load_gui_cache_payload() + entry = data.get("servo_targets") if isinstance(data, dict) else None + left = default_left + right = default_right + if isinstance(entry, dict): + l_val = entry.get("left") + r_val = entry.get("right") + if isinstance(l_val, (int, float)): + left = float(l_val) + if isinstance(r_val, (int, float)): + right = float(r_val) + return (left, right) + + +def _save_servo_targets_cache(left: float, right: float): + payload = _load_gui_cache_payload() + payload["servo_targets"] = { + "left": float(left), + "right": float(right), + "updated_at": time.strftime("%Y-%m-%d %H:%M:%S"), + } + _save_gui_cache_payload(payload) + + +def _run_remote_commands( + gui, + description: str, + commands, + *, + use_workspace_debug: bool = False, + target_robots=None, + allocate_tty: bool = False, +): + """Run commands over SSH for each target robot.""" + + selected_robots = target_robots if target_robots is not None else gui.get_selected_robots() + if isinstance(selected_robots, str): + selected_robots = [selected_robots] + if not selected_robots: + print(f"No robots selected. Skipping {description}.") + return + + cmd_list = list(commands) + if not cmd_list: + print(f"No commands provided for {description}.") + return + + workspace = (gui.get_workspace_name() or "").strip() + debug_prefix = _remote_debug_prefix(gui, workspace) if use_workspace_debug else "" + env_prefix = [ + "source ~/.bashrc", + f"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/", + "source /opt/ros/noetic/setup.bash", + ] + if workspace: + env_prefix.append(f"source ~/{workspace}/devel/setup.bash") + + for robot in selected_robots: + per_robot_cmds = [] + for cmd in cmd_list: + resolved_cmd = cmd(robot) if callable(cmd) else cmd + if not isinstance(resolved_cmd, str): + raise ValueError("Commands must resolve to strings") + resolved_cmd = resolved_cmd.strip() + if debug_prefix: + resolved_cmd = f"{debug_prefix}{resolved_cmd}" + per_robot_cmds.append(resolved_cmd) + + remote_cmd = "; ".join(env_prefix + per_robot_cmds) + ssh_cmd = ["ssh"] + if allocate_tty: + ssh_cmd += ["-t", "-t"] + ssh_cmd.extend([robot, remote_cmd]) + print(f"{description} on {robot} with command: {remote_cmd}") + stdin_target = None if allocate_tty else subprocess.DEVNULL + _popen_with_debug(ssh_cmd, gui, stdin=stdin_target) + + +def _kill_ros_nodes(node_names) -> bool: + """Kill the provided ROS nodes via rosnode kill, returns True if any ran.""" + nodes = sorted({n.strip() for n in node_names if n and n.strip()}) + if not nodes: + return False + + killed_any = False + for node in nodes: + try: + subprocess.check_call( + ["rosnode", "kill", node], + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + ) + killed_any = True + except (subprocess.CalledProcessError, FileNotFoundError): + print(f"Failed to kill node {node}. It may already be stopped.") + return killed_any + + +def _collect_driver_node_names(robot: str, launch_args) -> set: + """Use roslaunch introspection to list nodes defined for the driver launch.""" + cli_args = [arg for arg in launch_args if arg] + cmd = [ + "roslaunch", + "--nodes", + "mur_launch_hardware", + f"{robot}.launch", + ] + cli_args + + try: + output = subprocess.check_output(cmd, text=True, stderr=subprocess.DEVNULL) + except (subprocess.CalledProcessError, FileNotFoundError) as exc: + print(f"Failed to introspect nodes for {robot}: {exc}") + return set() + + nodes = set() + for line in output.splitlines(): + stripped = line.strip() + if not stripped or stripped.startswith("#"): + continue + nodes.add(stripped) + return nodes + + +def _load_driver_node_cache() -> dict: + """Load the persisted driver nodes from disk (per robot).""" + data = _load_gui_cache_payload() + + robots_section = data.get("robots") if isinstance(data, dict) else None + raw = robots_section if isinstance(robots_section, dict) else {} + + # legacy support: old payload stored under top-level driver_nodes/robots + if not raw: + legacy = data.get("driver_nodes") if isinstance(data, dict) else None + if isinstance(legacy, dict) and isinstance(legacy.get("robots"), dict): + raw = legacy.get("robots", {}) + elif isinstance(data, dict) and isinstance(data.get("robots"), dict): + raw = data.get("robots", {}) + + normalized = {} + for robot, nodes in raw.items(): + if not isinstance(robot, str): + continue + node_list = None + if isinstance(nodes, dict) and isinstance(nodes.get("driver_nodes"), (list, tuple, set)): + node_list = nodes.get("driver_nodes") + elif isinstance(nodes, (list, tuple, set)): + node_list = nodes + + if node_list: + normalized[robot] = set(str(n).strip() for n in node_list if str(n).strip()) + return normalized + + +def _save_driver_node_cache(node_map: dict): + """Persist the provided per-robot node map to disk as YAML.""" + payload = _load_gui_cache_payload() + robots_section = payload.setdefault("robots", {}) + timestamp = time.strftime("%Y-%m-%d %H:%M:%S") + + for robot, nodes in node_map.items(): + if not robot: + continue + normalized = sorted({str(n).strip() for n in nodes if str(n).strip()}) + robot_entry = robots_section.setdefault(robot, {}) + robot_entry["driver_nodes"] = normalized + robot_entry["updated_at"] = timestamp + + _save_gui_cache_payload(payload) + + +def _clamp_spray_distance(value: float) -> float: + return max(0.0, min(1.0, float(value))) + + +def _extract_spray_distance(entry) -> Optional[float]: + if isinstance(entry, dict): + block = entry.get("spray_distance") + if isinstance(block, dict): + val = block.get("value") + if isinstance(val, (int, float)): + return float(val) + if isinstance(block, (int, float)): + return float(block) + value = entry.get("value") + if isinstance(value, (int, float)): + return float(value) + elif isinstance(entry, (int, float)): + return float(entry) + return None + + +def _load_spray_distance_cache(robot: Optional[str] = None, default: float = DEFAULT_SPRAY_DISTANCE) -> float: + data = _load_gui_cache_payload() + + if robot: + robots_section = data.get("robots") if isinstance(data, dict) else None + if isinstance(robots_section, dict): + robot_entry = robots_section.get(robot) + value = _extract_spray_distance(robot_entry) + if value is not None: + return _clamp_spray_distance(value) + + # fallback to first available robot entry + robots_section = data.get("robots") if isinstance(data, dict) else None + if isinstance(robots_section, dict): + for entry in robots_section.values(): + value = _extract_spray_distance(entry) + if value is not None: + return _clamp_spray_distance(value) + + # fallback to legacy top-level spray_distance + value = _extract_spray_distance(data if isinstance(data, dict) else None) + if value is not None: + return _clamp_spray_distance(value) + + return default + + +def _save_spray_distance_cache(value: float, robots: Optional[Iterable[str]] = None): + payload = _load_gui_cache_payload() + robots_section = payload.setdefault("robots", {}) + timestamp = time.strftime("%Y-%m-%d %H:%M:%S") + clamped = float(_clamp_spray_distance(value)) + + robot_list = [r for r in (robots or []) if r] + for robot in robot_list: + entry = robots_section.setdefault(robot, {}) + entry["spray_distance"] = { + "value": clamped, + "updated_at": timestamp, + } + + payload["spray_distance"] = { + "value": clamped, + "updated_at": timestamp, + } + + _save_gui_cache_payload(payload) + + +def _normalize_path_index(value) -> Optional[int]: + try: + normalized = int(value) + except (TypeError, ValueError): + return None + return max(0, normalized) + + +def _load_path_index_cache(default: int = DEFAULT_PATH_INDEX) -> int: + data = _load_gui_cache_payload() + if isinstance(data, dict): + entry = data.get("path_index") + if isinstance(entry, dict): + for key in ("value", "default"): + normalized = _normalize_path_index(entry.get(key)) + if normalized is not None: + return normalized + else: + normalized = _normalize_path_index(entry) + if normalized is not None: + return normalized + normalized_default = _normalize_path_index(default) + return normalized_default if normalized_default is not None else DEFAULT_PATH_INDEX + + +def _save_path_index_cache(value: int): + normalized = _normalize_path_index(value) + if normalized is None: + return + payload = _load_gui_cache_payload() + payload["path_index"] = { + "value": normalized, + "updated_at": time.strftime("%Y-%m-%d %H:%M:%S"), + } + _save_gui_cache_payload(payload) + + +def _format_ros_list_arg(values): + """Format Python list as double-quoted string literal for roslaunch args.""" + return '"' + str(values).replace("'", '"') + '"' + + +def _load_component_choice(default: str = DEFAULT_COMPONENT_NAME) -> str: + data = _load_gui_cache_payload() + name = None + if isinstance(data, dict): + entry = data.get("component") + if isinstance(entry, dict): + raw = entry.get("name") or entry.get("value") + if isinstance(raw, str) and raw.strip(): + name = raw.strip() + elif isinstance(entry, str) and entry.strip(): + name = entry.strip() + elif isinstance(data.get("component_name"), str): + candidate = data.get("component_name", "") + if candidate.strip(): + name = candidate.strip() + + return name if name else default + + +def _save_component_choice(component_name: str): + normalized = (component_name or "").strip() + if not normalized: + return + payload = _load_gui_cache_payload() + payload["component"] = { + "name": normalized, + "updated_at": time.strftime("%Y-%m-%d %H:%M:%S"), + } + payload["component_name"] = normalized + _save_gui_cache_payload(payload) + + +def _default_component_transform(): + return {key: 0.0 for key in COMPONENT_TRANSFORM_KEYS} + + +def _coerce_float(value, fallback: float = 0.0) -> float: + if isinstance(value, (int, float)): + return float(value) + if isinstance(value, str): + try: + return float(value.strip()) + except ValueError: + return fallback + return fallback + + +def _normalize_component_transform(block) -> dict: + normalized = _default_component_transform() + if isinstance(block, dict): + for key in COMPONENT_TRANSFORM_KEYS: + normalized[key] = _coerce_float(block.get(key), normalized[key]) + return normalized + + +def _load_component_transform_entry(component_name: Optional[str]) -> dict: + name = (component_name or DEFAULT_COMPONENT_NAME).strip() or DEFAULT_COMPONENT_NAME + payload = _load_gui_cache_payload() + transforms = payload.get("component_transforms") if isinstance(payload, dict) else None + entry = {} + if isinstance(transforms, dict): + entry = transforms.get(name, {}) + return _normalize_component_transform(entry) + + +def _save_component_transform_entry(component_name: str, transform: dict): + name = (component_name or "").strip() + if not name: + return + payload = _load_gui_cache_payload() + transforms = payload.setdefault("component_transforms", {}) + normalized = _normalize_component_transform(transform) + stored = dict(normalized) + stored["updated_at"] = time.strftime("%Y-%m-%d %H:%M:%S") + transforms[name] = stored + _save_gui_cache_payload(payload) + + +class _DriverControlSession: + """Maintains a lightweight SSH shell per robot for reliable cleanup commands.""" + + def __init__(self, gui, robot: str, workspace: Optional[str]): + self.robot = robot + self.workspace = (workspace or "").strip() + self.process = _popen_with_debug( + ["ssh", "-T", robot, "bash", "-l"], + gui, + stdin=subprocess.PIPE, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + text=True, + ) + if self.process is not None and self.process.stdin is not None: + self._bootstrap_environment() + else: + self.process = None + + def _bootstrap_environment(self): + env_cmds = [ + "source ~/.bashrc", + f"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/", + "source /opt/ros/noetic/setup.bash", + ] + if self.workspace: + env_cmds.append(f"source ~/{self.workspace}/devel/setup.bash") + env_cmds.append("echo '[driver-control ready]' > /dev/null") + self.send(" && ".join(env_cmds)) + + def is_alive(self) -> bool: + return self.process is not None and self.process.poll() is None + + def send(self, command: str) -> bool: + if not self.is_alive() or not command: + return False + proc = self.process + if proc is None or proc.stdin is None: + return False + try: + proc.stdin.write(command.strip() + "\n") + proc.stdin.flush() + return True + except Exception: + return False + + def close(self): + proc = self.process + if not proc: + return + try: + self.send("exit") + if proc.stdin is not None: + proc.stdin.close() + except Exception: + pass + try: + proc.wait(timeout=2) + except Exception: + proc.kill() + + +def _ensure_driver_control_session(gui, robot: str, workspace: Optional[str]): + if gui is None: + return None + sessions = getattr(gui, "_driver_control_sessions", {}) + session = sessions.get(robot) + if session is not None and session.is_alive(): + return session + session = _DriverControlSession(gui, robot, workspace) + if session.is_alive(): + sessions[robot] = session + gui._driver_control_sessions = sessions + return session + return None + + +def _driver_kill_commands(robot: str): + return [ + f"pkill -f 'mur_launch_hardware {robot}.launch' || true", + "pkill -f ur_robot_driver || true", + "pkill -f roslaunch || true", + ] + + +def _driver_kill_script(robot: str) -> str: + return "; ".join(_driver_kill_commands(robot)) + + +def _stop_non_driver_commands(): + return [ + "pkill -f mir_trajectory_follower || true", + "pkill -f increment_path_index || true", + "pkill -f path_index_advancer || true", + "pkill -f complete_ur_trajectory_follower_ff_only || true", + "pkill -f 'ur_direction_controller|orthogonal_error_correction|move_ur_to_start_pose|ur_vel_induced_by_mir|world_twist_in_mir|twist_combiner|ur_yaw_controller' || true", + "pkill -f keyence_scanner_ljx8000 || true", + "pkill -f profile_orthogonal_controller || true", + "pkill -f strand-center-app || true", + "pkill -f parse_mir_path || true", + "pkill -f parse_ur_path || true", + "pkill -f target_broadcaster || true", + "pkill -f move_mir_to_start_pose || true", + "pkill -f move_ur_to_start_pose || true", + ] + + +def _stop_non_driver_script(_robot: str) -> str: + return "; ".join(_stop_non_driver_commands()) + +class ROSInterface: + def __init__(self, gui): + self.gui = gui + self.updated_poses = {} + self.virtual_object_pose = None + self.battery_states = {} + self.active_battery_subs = set() + self.current_index = _load_path_index_cache(DEFAULT_PATH_INDEX) + self._cached_path_index = self.current_index + self._cached_spray_distance = _load_spray_distance_cache() + self._cached_servo_targets = _load_servo_targets_cache() + self._cached_component_name = _load_component_choice(DEFAULT_COMPONENT_NAME) + self._component_transform_cache = {} + self._servo_state_lock = threading.Lock() + self._latest_servo_positions = {} + self._start_condition_topic = rospy.get_param("~start_condition_topic", "/start_condition") + self._start_signal_pub = None + self._start_signal_active = False + self._start_signal_timer = QTimer(self.gui) + self._start_signal_timer.setSingleShot(True) + self._start_signal_timer.setInterval(10000) + self._start_signal_timer.timeout.connect(self._deactivate_start_signal) + + # Rosbag config + self.rosbag_topics = [ + # "/tf", + "/ur_path_transformed", + "/mir_path_transformed", + "/profiles" + "/path_index", + "/laser_profile_offset_cmd_vel", + "/orthogonal_error", + "/orthogonal_twist", + "/ur_error_world" + "/mur620c/UR10_r/twist_controller/command_collision_free", + "/mur620c/UR10_r/twist_controller/controller_input", + "/ur_twist_direction_world", + "/servo_target_pos_left", + "/servo_target_pos_right", + "/mur620c/UR10_r/ur_calibrated_pose", + "/mur620c/UR10_r/global_tcp_pose", + "/mur620c/mir_pose_simple" + ] + self.rosbag_enabled = {t: True for t in self.rosbag_topics} + self.rosbag_process = None + self.rosbag_dir = os.path.expanduser("~/rosbags") # fixer Ordner + os.makedirs(self.rosbag_dir, exist_ok=True) + + + if not rospy.core.is_initialized(): + rospy.init_node("additive_manufacturing_gui", anonymous=True, disable_signals=True) + + # Subscriptions and publishers used by the GUI + rospy.Subscriber('/path_index', Int32, self._path_idx_cb, queue_size=10) + self._path_index_pub = rospy.Publisher('/path_index', Int32, queue_size=10, latch=False) + self._init_dynamixel_publishers() + self._init_servo_state_listener() + # Receive medians from robot-side node (published by profiles_median_node on the mur) + self._last_med_base = float('nan') + self._last_med_map = float('nan') + + def _median_base_cb(msg: Float32): + try: + self._last_med_base = float(msg.data) + except Exception: + self._last_med_base = float('nan') + try: + self.gui.medians.emit(self._last_med_base, self._last_med_map) + except Exception: + pass + + def _median_map_cb(msg: Float32): + try: + self._last_med_map = float(msg.data) + except Exception: + self._last_med_map = float('nan') + try: + self.gui.medians.emit(self._last_med_base, self._last_med_map) + except Exception: + pass + + try: + rospy.Subscriber('/profiles/median_base', Float32, _median_base_cb, queue_size=1) + rospy.Subscriber('/profiles/median_map', Float32, _median_map_cb, queue_size=1) + except Exception as e: + rospy.logwarn(f"Failed to subscribe to median topics: {e}") + + self._rosout_sub = None + self._subscribe_rosout_logs() + + def get_cached_spray_distance(self) -> float: + current = getattr(self, "_cached_spray_distance", DEFAULT_SPRAY_DISTANCE) + robots = getattr(self.gui, "get_selected_robots", lambda: [])() + target_robot = None + if isinstance(robots, str): + target_robot = robots + elif isinstance(robots, (list, tuple)) and robots: + target_robot = robots[0] + + resolved = _load_spray_distance_cache(target_robot, current) + self._cached_spray_distance = resolved + return resolved + + def persist_spray_distance(self, value: float, target_robots=None): + clamped = _clamp_spray_distance(value) + self._cached_spray_distance = clamped + + robots = target_robots if target_robots is not None else ( + getattr(self.gui, "get_selected_robots", lambda: [])() + ) + if isinstance(robots, str): + robots = [robots] + elif robots is None: + robots = [] + robots = [r for r in robots if r] + _save_spray_distance_cache(clamped, robots) + workspace = (getattr(self.gui, "get_workspace_name", lambda: "")() or "").strip() + + if not robots or not workspace: + return + + def _run_deploy(targets, ws): + for robot in targets: + try: + _deploy_gui_cache_to_robot(robot, ws) + except Exception as exc: + print(f"Failed to deploy GUI cache to {robot}: {exc}") + + threading.Thread(target=_run_deploy, args=(list(robots), workspace), daemon=True).start() + + def get_cached_servo_targets(self): + targets = getattr(self, "_cached_servo_targets", (0.0, 0.0)) + if isinstance(targets, (list, tuple)) and len(targets) == 2: + return targets + return (0.0, 0.0) + + def persist_servo_targets(self, left_percent: float, right_percent: float): + left = float(left_percent) + right = float(right_percent) + self._cached_servo_targets = (left, right) + _save_servo_targets_cache(left, right) + + self._subscribe_rosout_logs() + + def get_cached_path_index(self) -> int: + cached = getattr(self, "_cached_path_index", None) + if isinstance(cached, int): + return cached + value = _load_path_index_cache(self.current_index) + self._cached_path_index = value + return value + + def persist_path_index(self, value: int): + normalized = _normalize_path_index(value) + if normalized is None: + return + self._cached_path_index = normalized + self.current_index = normalized + _save_path_index_cache(normalized) + + def get_cached_component_name(self) -> str: + cached = getattr(self, "_cached_component_name", None) + if isinstance(cached, str) and cached.strip(): + return cached.strip() + resolved = _load_component_choice(DEFAULT_COMPONENT_NAME) + self._cached_component_name = resolved + return resolved + + def persist_component_choice(self, component_name: str): + normalized = (component_name or "").strip() + if not normalized: + return + self._cached_component_name = normalized + _save_component_choice(normalized) + + def get_component_transform(self, component_name: Optional[str] = None) -> dict: + name = (component_name or self.get_cached_component_name() or DEFAULT_COMPONENT_NAME).strip() + if not name: + name = DEFAULT_COMPONENT_NAME + cache = getattr(self, "_component_transform_cache", {}) or {} + if name not in cache: + cache[name] = _load_component_transform_entry(name) + self._component_transform_cache = cache + entry = cache.get(name, _default_component_transform()) + return dict(entry) + + def persist_component_transform(self, component_name: Optional[str], transform: dict): + name = (component_name or self.get_cached_component_name() or "").strip() + if not name: + return + normalized = _normalize_component_transform(transform) + cache = getattr(self, "_component_transform_cache", {}) or {} + cache[name] = normalized + self._component_transform_cache = cache + _save_component_transform_entry(name, normalized) + + def _subscribe_rosout_logs(self): + """Ensure we have a single /rosout subscription feeding the GUI.""" + if getattr(self, "_rosout_sub", None) is not None: + return + try: + self._rosout_sub = rospy.Subscriber('/rosout', Log, self._rosout_cb, queue_size=50) + except Exception as e: + self._rosout_sub = None + rospy.logwarn(f"Failed to subscribe to /rosout: {e}") + + def shutdown(self): + """Shut down background ROS helpers so the launcher terminal recovers.""" + if getattr(self, "_is_shutting_down", False): + return + self._is_shutting_down = True + + try: + if hasattr(self, "_start_signal_timer"): + self._start_signal_timer.stop() + except Exception: + pass + self._deactivate_start_signal() + + # Stop an active rosbag recording first so it flushes cleanly. + if self.rosbag_process and self.rosbag_process.poll() is None: + try: + self.rosbag_process.send_signal(signal.SIGINT) + self.rosbag_process.wait(timeout=4) + except Exception: + try: + self.rosbag_process.terminate() + self.rosbag_process.wait(timeout=2) + except Exception: + self.rosbag_process.kill() + finally: + self.rosbag_process = None + + # Tear down persistent SSH cleanup sessions to avoid dangling TTYs. + sessions = getattr(self.gui, "_driver_control_sessions", {}) or {} + for robot, session in list(sessions.items()): + try: + session.close() + except Exception as exc: + print(f"Failed to close driver cleanup session for {robot}: {exc}") + self.gui._driver_control_sessions = {} + + try: + rospy.signal_shutdown("GUI closed") + except Exception as exc: + print(f"Failed to signal rospy shutdown: {exc}") + + def _launch_process(self, command, shell=False, **kwargs): + return _popen_with_debug(command, self.gui, shell=shell, **kwargs) + + def _launch_in_terminal(self, title: str, command: str): + cmd = ["terminator", f"--title={title}", "-x", f"{command}; exec bash"] + return self._launch_process(cmd) + + def _debug_prefix_for_workspace(self, workspace: Optional[str] = None) -> str: + workspace = workspace or self.gui.get_workspace_name() + return _remote_debug_prefix(self.gui, workspace) + + def init_override_velocity_slider(self): + self.velocity_override_manual_pub = rospy.Publisher('/velocity_override_manual', Float32, queue_size=10, latch=True) + self.velocity_override_pub = rospy.Publisher('/velocity_override', Float32, queue_size=10, latch=True) + + def _handle_override_change(value: int): + self.gui.override_value_label.setText(f"{value}%") + if not self.gui.laser_override_radio.isChecked(): + self.velocity_override_pub.publish(value / 100.0) + else: + self.velocity_override_manual_pub.publish(value / 100.0) + + self.gui.override_slider.valueChanged.connect(_handle_override_change) + # publish to /velocity_override as well: + # ensure the initial slider value is sent once + self.velocity_override_pub.publish(self.gui.override_slider.value() / 100.0) + + def init_nozzle_override_slider(self): + """Wire the nozzle height override slider to /nozzle_height_override.""" + self.nozzle_height_override_pub = rospy.Publisher('/nozzle_height_override', Float32, queue_size=10, latch=True) + slider = getattr(self.gui, 'nozzle_override_slider', None) + label = getattr(self.gui, 'nozzle_override_value_label', None) + if slider is None or label is None: + return + + def _handle_change(raw_value: int): + meters = raw_value / 1000.0 # slider uses millimeters + label.setText(f"{raw_value:.1f} mm") + self.nozzle_height_override_pub.publish(Float32(meters)) + + slider.valueChanged.connect(_handle_change) + _handle_change(slider.value()) + + def trigger_start_signal(self): + publisher = self._ensure_start_signal_publisher() + if publisher is None: + rospy.logerr("Start signal publisher unavailable; cannot trigger start condition.") + return + + try: + publisher.publish(Bool(data=True)) + except Exception as exc: + rospy.logerr(f"Failed to publish start signal: {exc}") + return + + self._start_signal_active = True + if hasattr(self.gui, "update_start_signal_visual"): + self.gui.update_start_signal_visual(True) + if hasattr(self, "_start_signal_timer"): + self._start_signal_timer.start() + + def _ensure_start_signal_publisher(self): + if self._start_signal_pub is not None: + return self._start_signal_pub + try: + self._start_signal_pub = rospy.Publisher( + self._start_condition_topic, + Bool, + queue_size=1, + latch=True, + ) + except Exception as exc: + rospy.logerr(f"Failed to create start signal publisher: {exc}") + self._start_signal_pub = None + return self._start_signal_pub + + def _deactivate_start_signal(self): + publisher = getattr(self, "_start_signal_pub", None) + if publisher is not None: + try: + publisher.unregister() + except Exception as exc: + rospy.logwarn(f"Failed to unregister start signal publisher: {exc}") + finally: + self._start_signal_pub = None + + if self._start_signal_active: + self._start_signal_active = False + if hasattr(self.gui, "update_start_signal_visual"): + self.gui.update_start_signal_visual(False) + + def _path_idx_cb(self, msg: Int32): + self.current_index = msg.data + self._cached_path_index = self.current_index + self.gui.path_idx.emit(self.current_index) # Update the GUI with the new index + + def publish_path_index(self, value: int): + """Publish the provided index on /path_index so downstream nodes follow the change.""" + try: + normalized = int(value) + except (TypeError, ValueError): + rospy.logwarn(f"Ignoring invalid path index value: {value}") + return + + publisher = getattr(self, "_path_index_pub", None) + if publisher is None: + try: + publisher = rospy.Publisher('/path_index', Int32, queue_size=10, latch=True) + self._path_index_pub = publisher + except Exception as exc: + rospy.logerr(f"Failed to create path index publisher: {exc}") + return + + try: + publisher.publish(Int32(data=normalized)) + self.current_index = normalized + self._cached_path_index = normalized + rospy.loginfo(f"Published path index {normalized}") + except Exception as exc: + rospy.logerr(f"Failed to publish path index {normalized}: {exc}") + + def start_roscore(self): + """Starts roscore on the roscore PC.""" + workspace = self.gui.get_workspace_name() + debug_prefix = self._debug_prefix_for_workspace(workspace) + command = ( + "ssh -t -t roscore 'source ~/.bashrc; source /opt/ros/noetic/setup.bash; " + f"{debug_prefix}roscore; exec bash'" + ) + self._launch_in_terminal("Roscore Terminal", command) + + def start_mocap(self): + """Starts the motion capture system on the roscore PC.""" + workspace = self.gui.get_workspace_name() + debug_prefix = self._debug_prefix_for_workspace(workspace) + command = ( + "ssh -t -t roscore 'source ~/.bashrc; source /opt/ros/noetic/setup.bash; " + "source ~/catkin_ws/devel/setup.bash; " + f"{debug_prefix}roslaunch launch_mocap mocap_launch.launch; exec bash'" + ) + self._launch_in_terminal("Mocap", command) + + def start_sync(self): + """Starts file synchronization between workspace and selected robots.""" + selected_robots = self.gui.get_selected_robots() + self.workspace_name = self.gui.get_workspace_name() + self.gui.btn_sync.setStyleSheet("background-color: lightgreen;") # Mark sync as active + + for robot in selected_robots: + command = f"while inotifywait -r -e modify,create,delete,move ~/{self.workspace_name}/src; do \n" \ + f"rsync --delete -avzhe ssh ~/{self.workspace_name}/src rosmatch@{robot}:~/{self.workspace_name}/ \n" \ + "done" + self._launch_in_terminal(f"Sync to {robot}", command) + + def prime_driver_cleanup_sessions(self): + """Manually open/refresh the persistent SSH session used for driver cleanup.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robots selected. Skipping driver cleanup session setup.") + return + + workspace = self.gui.get_workspace_name() + ready = [] + failed = [] + + for robot in selected_robots: + session = _ensure_driver_control_session(self.gui, robot, workspace) + if session and session.is_alive(): + ready.append(robot) + else: + failed.append(robot) + + if ready: + print(f"Driver cleanup channel ready for: {', '.join(ready)}") + if failed: + print(f"Failed to open driver cleanup channel for: {', '.join(failed)}") + + def update_button_status(self): + # --- Parser + Keyence status --- + node_cache = self._get_rosnode_list() + mir = self.is_ros_node_running_fast("/retrieve_and_publish_mir_path", node_cache) + ur = self.is_ros_node_running_fast("/retrieve_and_publish_ur_path", node_cache) + key = self.is_ros_node_running_fast("/keyence_ljx_profile_node", node_cache) + flow = self.is_ros_node_running_fast("/flow_serial_bridge", node_cache) + tgt = self.is_ros_node_running_fast("/target_broadcaster", node_cache) + laser = self.is_ros_node_running_fast("/laser_profile_controller", node_cache) + mocap = self.is_ros_node_running_fast("/qualisys", node_cache) + + # --- Button coloring --- + self.gui.btn_parse_mir.setStyleSheet("background-color: lightgreen;" if mir else "background-color: lightgray;") if hasattr(self.gui,"btn_parse_mir") else None + self.gui.btn_parse_ur.setStyleSheet("background-color: lightgreen;" if ur else "background-color: lightgray;") if hasattr(self.gui,"btn_parse_ur") else None + self.gui.btn_keyence.setStyleSheet("background-color: lightgreen;" if key else "background-color: lightgray;") if hasattr(self.gui,"btn_keyence") else None + if hasattr(self.gui, "btn_flow_sensor"): + self.gui.btn_flow_sensor.setStyleSheet("background-color: lightgreen;" if flow else "background-color: lightgray;") + self.gui.btn_target_broadcaster.setStyleSheet("background-color: lightgreen;" if tgt else "background-color: lightgray;") if hasattr(self.gui,"btn_target_broadcaster") else None + self.gui.btn_laser_ctrl.setStyleSheet("background-color: lightgreen;" if laser else "background-color: lightgray;") if hasattr(self.gui,"btn_laser_ctrl") else None + self.gui.btn_mocap.setStyleSheet("background-color: lightgreen;" if mocap else "background-color: lightgray;") if hasattr(self.gui,"btn_mocap") else None + + # --- Driver status (UR hardware interface nodes) --- + driver_nodes = self._driver_node_names( + robots=self.gui.get_selected_robots(), + urs=self.gui.get_selected_urs(), + ) + driver_total = len(driver_nodes) + driver_active = sum(1 for node in driver_nodes if node in node_cache) + if hasattr(self.gui, "btn_launch_drivers"): + if driver_total == 0 or driver_active == 0: + driver_color = "background-color: lightgray;" + elif driver_active == driver_total: + driver_color = "background-color: lightgreen;" + else: + driver_color = "background-color: orange;" + self.gui.btn_launch_drivers.setStyleSheet(driver_color) + + # --- Auto-start target broadcaster --- + if mir and ur and not tgt: + print("Auto-starting /target_broadcaster…"); _popen_with_debug(f"roslaunch print_hw target_broadcaster.launch initial_path_index:={self.gui.idx_spin.value()}", self.gui, shell=True) + + def _get_rosnode_list(self): + """Return the current ROS node names as a set for quick membership checks.""" + try: + output = subprocess.check_output(["rosnode", "list"], text=True) + except (subprocess.CalledProcessError, FileNotFoundError): + return set() + return {line.strip() for line in output.splitlines() if line.strip()} + + def is_ros_node_running(self, node_name, node_cache=None): + """Checks if a specific ROS node is running by using `rosnode list`.""" + nodes = node_cache if node_cache is not None else self._get_rosnode_list() + return node_name in nodes + + def is_ros_node_running_fast(self, name, node_cache=None): + if node_cache is None: + node_cache = self._get_rosnode_list() + return name in node_cache + + def _driver_node_names(self, robots=None, urs=None): + """Return the UR driver node names to monitor for the given robot/UR selection.""" + robot_names = robots if robots else list(getattr(self.gui, "robots", {}).keys()) + if not robot_names: + return [] + ur_prefixes = urs if urs else ["UR10_l", "UR10_r"] + names = [] + for robot in robot_names: + for ur in ur_prefixes: + names.append(f"/{robot}/{ur}/ur_hardware_interface") + return names + + def make_ur_callback(self, robot): + def callback(msg): + if robot not in self.battery_states: + self.battery_states[robot] = {} + self.battery_states[robot]["ur"] = msg.data + self.update_battery_display(robot) + return callback + + def make_mir_callback(self, robot): + def callback(msg): + if robot not in self.battery_states: + self.battery_states[robot] = {} + self.battery_states[robot]["mir"] = msg.percentage * 100 # falls 0.0–1.0 + self.update_battery_display(robot) + return callback + + def update_battery_display(self, robot): + mir_label, ur_label = self.gui.battery_labels.get(robot, (None, None)) + if not mir_label or not ur_label: + return + + state = self.battery_states.get(robot, {}) + + if "mir" in state: + val = state["mir"] + mir_label.setText(f"MiR: {val:.0f}%") + mir_label.setStyleSheet(f"background-color: {self.get_color(val)}; padding: 2px;") + + if "ur" in state: + val = state["ur"] + ur_label.setText(f"UR: {val:.0f}%") + ur_label.setStyleSheet(f"background-color: {self.get_color(val)}; padding: 2px;") + + + def check_and_subscribe_battery(self): + if not rospy.core.is_initialized(): + rospy.init_node("battery_status_gui", anonymous=True, disable_signals=True) + + for robot in self.gui.get_selected_robots(): + if robot in self.active_battery_subs: + continue # bereits abonniert + + rospy.Subscriber(f"/{robot}/bms_status/SOC", Float32, self.make_ur_callback(robot), queue_size=1) + rospy.Subscriber(f"/{robot}/battery_state", BatteryState, self.make_mir_callback(robot)) + self.active_battery_subs.add(robot) + print(f"🔌 Subscribed to battery topics for {robot}") + + def get_color(self, val): + if val >= 50: + return "lightgreen" + elif val >= 20: + return "orange" + else: + return "red" + + def launch_keyence_scanner(self): + """Launches the Keyence scanner on the robots PC.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping Keyence scanner launch.") + return + + _run_remote_commands( + self.gui, + "Launching Keyence scanner", + ["roslaunch laser_scanner_tools keyence_scanner_ljx8000.launch"], + use_workspace_debug=True, + target_robots=selected_robots, + ) + + def launch_flow_sensor_bridge(self): + """Launches the foam volume flow sensor bridge on the robots.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping flow sensor bridge launch.") + return + + _run_remote_commands( + self.gui, + "Launching flow sensor bridge", + ["roslaunch foam_volume_flow_sensor flow_serial_bridge.launch"], + use_workspace_debug=True, + target_robots=selected_robots, + ) + + def stop_flow_sensor_bridge(self): + """Stops the foam volume flow sensor bridge on the robots.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping flow sensor bridge stop.") + return + + stop_cmds = [ + "rosnode kill /flow_serial_bridge || true", + "pkill -f flow_serial_bridge || true", + ] + + _run_remote_commands( + self.gui, + "Stopping flow sensor bridge", + stop_cmds, + use_workspace_debug=True, + target_robots=selected_robots, + ) + + def launch_laser_orthogonal_controller(self): + """Launches the Keyence scanner on the robots PC.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping orthogonal controller launch.") + return + + _run_remote_commands( + self.gui, + "Launching orthogonal controller", + ["roslaunch laser_scanner_tools profile_orthogonal_controller.launch"], + use_workspace_debug=True, + target_robots=selected_robots, + ) + + def _orth_pid_config_path(self) -> str: + try: + pkg_path = rospkg.RosPack().get_path("ur_trajectory_follower") + return os.path.join(pkg_path, "config", "pid_twist_controller_direction_orthogonal.yaml") + except Exception as exc: + print(f"Falling back to relative orthogonal PID path: {exc}") + return os.path.abspath( + os.path.join( + os.path.dirname(__file__), + "..", + "..", + "ur_trajectory_follower", + "config", + "pid_twist_controller_direction_orthogonal.yaml", + ) + ) + + def _remote_orth_pid_config_path(self, workspace: Optional[str]) -> str: + workspace = (workspace or "").strip() + if workspace: + return f"~/{workspace}/src/match_additive_manufacturing/ur_trajectory_follower/config/pid_twist_controller_direction_orthogonal.yaml" + return self._orth_pid_config_path() + + def load_orthogonal_pid_config(self) -> dict: + path = self._orth_pid_config_path() + with open(path, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + return data if isinstance(data, dict) else {} + + def _push_orth_pid_config_to_robots(self, local_path: str): + workspace = (self.gui.get_workspace_name() or "").strip() + robots = self.gui.get_selected_robots() + if not workspace or not robots: + return + remote_path = self._remote_orth_pid_config_path(workspace) + for robot in robots: + try: + subprocess.check_call(["scp", local_path, f"{robot}:{remote_path}"]) + except subprocess.CalledProcessError as exc: + print(f"Failed to copy PID config to {robot}: {exc}") + + def save_orthogonal_pid_config(self, payload: dict): + path = self._orth_pid_config_path() + os.makedirs(os.path.dirname(path), exist_ok=True) + with open(path, "w", encoding="utf-8") as stream: + yaml.safe_dump(payload or {}, stream, default_flow_style=False, sort_keys=False) + self._push_orth_pid_config_to_robots(path) + + def start_orthogonal_pid_controller(self): + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping orthogonal PID start.") + return + + workspace = (self.gui.get_workspace_name() or "").strip() + if workspace: + cfg_arg = ( + f"$HOME/{workspace}/src/match_additive_manufacturing/ur_trajectory_follower/config/" + "pid_twist_controller_direction_orthogonal.yaml" + ) + else: + cfg_arg = "$(rospack find ur_trajectory_follower)/config/pid_twist_controller_direction_orthogonal.yaml" + + def _launch_cmd(_robot): + return ( + "roslaunch additive_manufacturing_helpers pid_twist_controller.launch " + "node_name:=pid_twist_controller_orthogonal " + "input_twist_topic:=/orthogonal_error " + "output_twist_topic:=/orthogonal_twist " + f"pid_values_path:={cfg_arg}" + ) + + _run_remote_commands( + self.gui, + "Starting orthogonal PID controller", + [_launch_cmd], + use_workspace_debug=True, + target_robots=selected_robots, + ) + + def stop_orthogonal_pid_controller(self): + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping orthogonal PID stop.") + return + + stop_cmds = [ + "rosnode kill /pid_twist_controller_orthogonal/pid_twist_controller_orthogonal || true", + "rosnode kill /pid_twist_controller_orthogonal || true", + "pkill -f pid_twist_controller_orthogonal || true", + ] + + _run_remote_commands( + self.gui, + "Stopping orthogonal PID controller", + stop_cmds, + use_workspace_debug=True, + target_robots=selected_robots, + ) + + def launch_strand_center_app(self): + """Launch the DepthAI strand-center app on the selected robots over SSH.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping strand-center launch.") + return + + workspace = self.gui.get_workspace_name() + script_rel = "src/match_additive_manufacturing/camera_oak/strand-center-app/main.py" + + for robot in selected_robots: + remote_script = f"~/{workspace}/{script_rel}" + debug_prefix = self._debug_prefix_for_workspace(workspace) + command = ( + f"ssh -t -t {robot} '" + "source ~/.bashrc; " + f"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/; " + "source /opt/ros/noetic/setup.bash; " + f"source ~/{workspace}/devel/setup.bash; " + f"{debug_prefix}python3 {remote_script}; " + "exec bash'" + ) + + self._launch_in_terminal(f"StrandCenter {robot}", command) + + def start_local_rosbag(self, fname, topics): + cmd = ["rosbag", "record", "-O", fname, "--lz4"] + topics + print("Starting LOCAL rosbag:", " ".join(cmd)) + proc = subprocess.Popen(cmd) + self._local_rosbag_proc = proc + return proc + + def stop_local_rosbag(self): + proc = getattr(self, "_local_rosbag_proc", None) + if not proc or proc.poll() is not None: + return + print("Stopping LOCAL rosbag…") + try: + proc.send_signal(signal.SIGINT); proc.wait(timeout=4) + except: + proc.terminate() + self._local_rosbag_proc = None + + def start_remote_rosbag(self, gui, robot, fname, topics): + pidfile = f"/home/rosmatch/rosbags/rosbag_{robot}.pid" + rosbag_cmd = f"rosbag record -O {fname} --lz4 {' '.join(topics)}" + + remote_cmd = ( + f"ssh -t -t {robot} '" + "source ~/.bashrc; " + f"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/; " + "source /opt/ros/noetic/setup.bash; " + "source ~/catkin_ws/devel/setup.bash; " + "mkdir -p ~/rosbags; " + f"nohup {rosbag_cmd} > /home/rosmatch/rosbags/rosbag_{robot}.log 2>&1 & " + "echo $! > " + pidfile + "; " + "exec bash'" + ) + + print("Starting REMOTE rosbag:", remote_cmd) + proc = _popen_with_debug([ + "terminator", + f"--title=Rosbag {robot}", + "-x", + f"{remote_cmd}; exec bash" + ], gui) + + if not hasattr(gui, "_remote_rosbag_procs"): + gui._remote_rosbag_procs = {} + gui._remote_rosbag_procs[robot] = proc + return proc + + + def stop_remote_rosbag(self, gui, robot): + pidfile = f"/home/rosmatch/rosbags/rosbag_{robot}.pid" + stop_cmd = ( + f"ssh -t -t {robot} '" + f"if [ -f {pidfile} ]; then " + f" pid=$(cat {pidfile}); " + f" echo Stopping rosbag pid $pid; " + f" kill -2 $pid || true; " + f" sleep 2; " + f" kill $pid || true; " + f" rm {pidfile}; " + "fi; " + "exit'" + ) + print(f"Stopping REMOTE rosbag on {robot}…") + subprocess.Popen(stop_cmd, shell=True) + + # mark process as stopped in GUI + if hasattr(gui, "_remote_rosbag_procs"): + gui._remote_rosbag_procs[robot] = None + + + + def toggle_rosbag_record(self, gui): + # rebuild topic selection + local_topics = [t for t, s in gui.topic_settings.items() if s["local"]] + remote_topics = [t for t, s in gui.topic_settings.items() if s["remote"]] + + # check running state + local_running = hasattr(self, "_local_rosbag_proc") and self._local_rosbag_proc and self._local_rosbag_proc.poll() is None + remote_running = hasattr(gui, "_remote_rosbag_procs") and any( + p and p.poll() is None for p in gui._remote_rosbag_procs.values() + ) + + # --- STOP --- + if local_running or remote_running: + self.stop_local_rosbag() + if hasattr(gui, "_remote_rosbag_procs"): + for robot in gui._remote_rosbag_procs: + self.stop_remote_rosbag(gui, robot) + + gui.btn_rosbag_record.setStyleSheet("background-color: lightgray;") + return + + # --- START --- + ts = time.strftime("%Y%m%d_%H%M%S") + local_fname = f"{self.rosbag_dir}/record_{ts}_GUI-PC" + remote_fname = f"~/rosbags/record_{ts}_MuR" + + robot_list = gui.get_selected_robots() or ["mur620c"] + + if local_topics: + self.start_local_rosbag(local_fname, local_topics) + + if remote_topics: + for robot in robot_list: + self.start_remote_rosbag(gui, robot, remote_fname, remote_topics) + + gui.btn_rosbag_record.setStyleSheet("background-color: red; color: white;") + + + # -------------------- Dynamixel Driver & Servo Targets -------------------- + def start_dynamixel_driver(self): + """Start the Dynamixel servo driver on the selected robots over SSH (new terminal per robot).""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping Dynamixel start.") + return + workspace = self.gui.get_workspace_name() + for robot in selected_robots: + debug_prefix = self._debug_prefix_for_workspace(workspace) + command = ( + f"ssh -t -t {robot} '" + f"source ~/.bashrc; " + f"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/; " + f"source /opt/ros/noetic/setup.bash; " + f"source ~/{workspace}/devel/setup.bash; " + f"{debug_prefix}roslaunch dynamixel_match dynamixel_motors.launch; exec bash'" + ) + self._launch_in_terminal(f"Dynamixel {robot}", command) + + def stop_dynamixel_driver(self): + """Stop the Dynamixel servo driver on the selected robots over SSH.""" + selected_robots = self.gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping Dynamixel stop.") + return + workspace = self.gui.get_workspace_name() + for robot in selected_robots: + # Kill by rosnode and fall back to process patterns + remote_cmd = ( + "source ~/.bashrc; " + F"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/; " + "source /opt/ros/noetic/setup.bash; " + f"source ~/{workspace}/devel/setup.bash; " + "rosnode kill /servo_driver || true; " + "pkill -f dynamixel_motors.launch || true; " + "pkill -f servo_driver.py || true; " + "pkill -f dynamixel_workbench_controllers || true;" + ) + ssh_cmd = ["ssh", robot, remote_cmd] + _popen_with_debug(ssh_cmd, self.gui, stdin=subprocess.DEVNULL) + + def _init_dynamixel_publishers(self): + """Initialize publishers for left/right servo target topics (Int16).""" + try: + self.servo_left_pub = rospy.Publisher('/servo_target_pos_left', Int16, queue_size=10) + self.servo_right_pub = rospy.Publisher('/servo_target_pos_right', Int16, queue_size=10) + except Exception as e: + rospy.logwarn(f"Failed to create Dynamixel target publishers: {e}") + + def publish_servo_targets(self, left_value: int, right_value: int): + """Publish target positions for both servos as Int16 (0..4095). + Values are forwarded as std_msgs/Int16 on /servo_target_pos_left and /servo_target_pos_right. + """ + if not hasattr(self, 'servo_left_pub'): + self._init_dynamixel_publishers() + try: + # Coerce to servo range 0..4095 + left_int = max(min(int(round(left_value)), 4095), 0) + right_int = max(min(int(round(right_value)), 4095), 0) + self.servo_left_pub.publish(Int16(left_int)) + self.servo_right_pub.publish(Int16(right_int)) + rospy.loginfo(f"Published servo targets L={left_int}, R={right_int}") + except Exception as e: + rospy.logerr(f"Failed to publish servo targets: {e}") + + def _init_servo_state_listener(self): + """Subscribe to the Dynamixel state topic to cache latest raw positions.""" + try: + rospy.Subscriber( + '/dynamixel_workbench/dynamixel_state', + DynamixelStateList, + self._servo_state_cb, + queue_size=1, + ) + except Exception as exc: + rospy.logwarn(f"Failed to subscribe to dynamixel state topic: {exc}") + + def _servo_state_cb(self, msg: DynamixelStateList): + entries = getattr(msg, 'dynamixel_state', []) if msg is not None else [] + if not entries: + return + with self._servo_state_lock: + store = self._latest_servo_positions + for state in entries: + try: + pos_val = int(round(getattr(state, 'present_position', 0))) + except Exception: + continue + keys = set() + name = getattr(state, 'name', '') + if isinstance(name, str) and name.strip(): + normalized = name.strip().lower() + keys.add(normalized) + if normalized.startswith('servo_'): + keys.add(normalized.replace('servo_', '', 1)) + keys.add(f"servo_{normalized}") + try: + motor_id = int(getattr(state, 'id', -1)) + if motor_id >= 0: + keys.add(str(motor_id)) + keys.add(f"id_{motor_id}") + except Exception: + pass + for key in keys: + store[key] = pos_val + + def get_latest_servo_position(self, which: str): + """Return the most recent raw position for the requested servo.""" + if not which: + return None + lookup_keys = [] + normalized = str(which).strip().lower() + if normalized: + lookup_keys.extend([ + normalized, + f"servo_{normalized}", + normalized.replace('servo_', '', 1) if normalized.startswith('servo_') else '', + ]) + if normalized in ('left', 'right'): + lookup_keys.append('1' if normalized == 'left' else '2') + with self._servo_state_lock: + snapshot = dict(self._latest_servo_positions) + for key in lookup_keys: + if key and key in snapshot: + return snapshot[key] + return None + + def _rosout_cb(self, msg: Log): + """ + Callback for /rosout (rosgraph_msgs/Log). + Forwards a compact text line to the Qt GUI via signal. + """ + try: + text = getattr(msg, "msg", "") + + # --- Blacklist bestimmter Meldungen --- + blacklist = [ + "The complete state of the robot is not yet known", + "Battery:", + "Unable to transform object from frame", + "tf_static: sent 17 transforms", + "tf_static: updated transform", + # hier ggf. weitere Phrasen ergänzen + ] + for phrase in blacklist: + if phrase in text: + return # einfach ignorieren + + # Map level to readable string (if constants exist) + level_map = { + getattr(Log, "DEBUG", 1): "DEBUG", + getattr(Log, "INFO", 2): "INFO", + getattr(Log, "WARN", 4): "WARN", + getattr(Log, "ERROR", 8): "ERROR", + getattr(Log, "FATAL", 16): "FATAL", + } + level = level_map.get(msg.level, str(msg.level)) + node_name = getattr(msg, "name", "?") + + if hasattr(self.gui, "ros_log_signal"): + self.gui.ros_log_signal.emit(level, node_name, text) + except Exception as e: + rospy.logwarn(f"Error while forwarding /rosout message: {e}") + + + +def launch_ros(gui, package, launch_file): + selected_robots = gui.get_selected_robots() + robot_names_str = "[" + ",".join(f"'{r}'" for r in selected_robots) + "]" + + command = f"roslaunch {package} {launch_file} robot_names:={robot_names_str}" + print(f"Executing: {command}") + _popen_with_debug(command, gui, shell=True) + +def start_status_update(gui): + threading.Thread(target=update_status, args=(gui,), daemon=True).start() + +def update_status(gui): + selected_robots = gui.get_selected_robots() + selected_urs = gui.get_selected_urs() + active_counts = {"force_torque_sensor_controller": 0, "twist_controller": 0, "arm_controller": 0, "admittance": 0} + total_count = len(selected_robots) * len(selected_urs) + + for robot in selected_robots: + for ur in selected_urs: + service_name = f"/{robot}/{ur}/controller_manager/list_controllers" + try: + output = subprocess.check_output(f"rosservice call {service_name}", shell=True).decode() + controllers = yaml.safe_load(output).get("controller", []) + for controller in controllers: + if controller.get("state") == "running": + active_counts[controller["name"]] = active_counts.get(controller["name"], 0) + 1 + except Exception as e: + rospy.logwarn(f"Error checking controllers for {robot}/{ur}: {e}") + + status_text = """ + Force/Torque Sensor: {}/{} {} + Twist Controller: {}/{} {} + Arm Controller: {}/{} {} + Admittance Controller: {}/{} {} + """.format( + active_counts["force_torque_sensor_controller"], total_count, get_status_symbol(active_counts["force_torque_sensor_controller"], total_count), + active_counts["twist_controller"], total_count, get_status_symbol(active_counts["twist_controller"], total_count), + active_counts["arm_controller"], total_count, get_status_symbol(active_counts["arm_controller"], total_count), + active_counts["admittance"], total_count, get_status_symbol(active_counts["admittance"], total_count), + ) + + gui.status_label.setText(status_text) + +def get_status_symbol(active, total): + if active == total: + return "✅" + elif active > 0: + return "⚠️" + return "❌" + +def open_rviz(gui): + command = "roslaunch print_gui launch_rviz.launch" + _popen_with_debug(command, gui, shell=True) + +def turn_on_arm_controllers(gui): + """Turns on all arm controllers for the selected robots.""" + selected_robots = gui.get_selected_robots() + selected_urs = gui.get_selected_urs() + + if not selected_robots or not selected_urs: + print("No robots or URs selected. Skipping launch.") + return + + ur_prefixes_str = _format_ros_list_arg(selected_urs) + + for robot in selected_robots: + robot_list_str = _format_ros_list_arg([robot]) + _run_remote_commands( + gui, + f"Turning on arm controllers", + [ + ( + f"roslaunch print_gui turn_on_all_arm_controllers.launch " + f"robot_names:={robot_list_str} UR_prefixes:={ur_prefixes_str}" + ) + ], + use_workspace_debug=True, + target_robots=[robot], + ) + +def turn_on_twist_controllers(gui): + """Turns on all twist controllers for the selected robots.""" + selected_robots = gui.get_selected_robots() + selected_urs = gui.get_selected_urs() + + if not selected_robots or not selected_urs: + print("No robots or URs selected. Skipping launch.") + return + + ur_prefixes_str = _format_ros_list_arg(selected_urs) + + for robot in selected_robots: + robot_list_str = _format_ros_list_arg([robot]) + _run_remote_commands( + gui, + "Turning on twist controllers", + [ + ( + f"roslaunch print_gui turn_on_all_twist_controllers.launch " + f"robot_names:={robot_list_str} UR_prefixes:={ur_prefixes_str}" + ) + ], + use_workspace_debug=True, + target_robots=[robot], + ) + +def enable_all_urs(gui): + """Enables all UR robots for the selected configurations.""" + selected_robots = gui.get_selected_robots() + selected_urs = gui.get_selected_urs() + + if not selected_robots or not selected_urs: + print("No robots or URs selected. Skipping launch.") + return + + ur_prefixes_str = _format_ros_list_arg(selected_urs) + + for robot in selected_robots: + robot_list_str = _format_ros_list_arg([robot]) + _run_remote_commands( + gui, + "Enabling URs", + [ + ( + f"roslaunch print_gui enable_all_URs.launch " + f"robot_names:={robot_list_str} UR_prefixes:={ur_prefixes_str}" + ) + ], + use_workspace_debug=True, + target_robots=[robot], + ) + + +def launch_drivers(gui): + """SSH into the selected robots, start drivers, and keep a cleanup channel per robot.""" + selected_robots = gui.get_selected_robots() + workspace_name = gui.get_workspace_name() + + if not selected_robots: + print("No robots selected. Skipping driver launch.") + return + + if not hasattr(gui, "_driver_processes"): + gui._driver_processes = [] + gui._driver_processes = [p for p in gui._driver_processes if p and p.poll() is None] + gui._driver_robots = list(set(selected_robots)) + + selected_urs = gui.get_selected_urs() + try: + offset_values = gui.get_tcp_offset_sixd() + except AttributeError: + offset_values = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + tcp_offset_literal = "[" + ",".join(f"{value:.6f}" for value in offset_values) + "]" + + launch_arg_pairs = [ + ("launch_ur_l", "true" if "UR10_l" in selected_urs else "false"), + ("launch_ur_r", "true" if "UR10_r" in selected_urs else "false"), + ("tcp_offset", tcp_offset_literal), + ] + launch_cli_args = [f"{name}:={value}" for name, value in launch_arg_pairs] + launch_suffix = "".join(f" {arg}" for arg in launch_cli_args) + persist_updates = {} + + for robot in selected_robots: + workspace = workspace_name + + debug_prefix = _remote_debug_prefix(gui, workspace) + command = ( + f"ssh -t -t {robot} '" + "source ~/.bashrc; " + f"export ROS_MASTER_URI=http://{ROSCORE_HOST}:11311/; " + "source /opt/ros/noetic/setup.bash; " + f"source ~/{workspace}/devel/setup.bash; " + f"{debug_prefix}roslaunch mur_launch_hardware {robot}.launch{launch_suffix}; " + "exec bash'" + ) + + proc = _popen_with_debug([ + "terminator", + f"--title=Driver {robot}", + "-x", + f"{command}; exec bash" + ], gui) + if proc is not None: + gui._driver_processes.append(proc) + + _ensure_driver_control_session(gui, robot, workspace) + + nodes = _collect_driver_node_names(robot, launch_cli_args) + if nodes: + node_map = getattr(gui, "_driver_nodes_by_robot", {}) + node_map[robot] = nodes + gui._driver_nodes_by_robot = node_map + persist_updates[robot] = nodes + + if persist_updates: + cache = _load_driver_node_cache() + cache.update({robot: set(nodes) for robot, nodes in persist_updates.items()}) + _save_driver_node_cache(cache) + for robot in selected_robots: + _deploy_gui_cache_to_robot(robot, workspace_name) + +def quit_drivers(gui=None): + """Terminates running driver sessions and uses the per-robot SSH channel for clean stops.""" + print("Stopping all driver sessions...") + workspace_name = gui.get_workspace_name() if gui is not None else None + processes = [] + if gui is not None and hasattr(gui, "_driver_processes"): + processes = [p for p in gui._driver_processes if p and p.poll() is None] + + for proc in processes: + try: + proc.terminate() + proc.wait(timeout=2) + except subprocess.TimeoutExpired: + proc.kill() + except Exception as exc: + rospy.logwarn(f"Failed to terminate driver terminal: {exc}") + + if gui is not None: + gui._driver_processes = [p for p in getattr(gui, "_driver_processes", []) if p and p.poll() is None] + + # Ask the robots themselves to stop the driver launch files. + robots = [] + if gui is not None: + robots = gui.get_selected_robots() + if not robots: + robots = getattr(gui, "_driver_robots", []) + + if gui is not None and robots: + node_map = getattr(gui, "_driver_nodes_by_robot", {}) + file_cache = _load_driver_node_cache() + for robot, nodes in file_cache.items(): + node_map.setdefault(robot, set()).update(nodes) + + target_nodes = set() + for robot in robots: + target_nodes.update(node_map.get(robot, set())) + + if target_nodes and _kill_ros_nodes(target_nodes): + print("Killed driver nodes derived from launch introspection.") + # Keep cache entries so other operators can still reference the + # launch configuration even after this GUI session shuts it down. + combined_cache = {robot: set(nodes) for robot, nodes in node_map.items() if nodes} + if file_cache: + for robot, nodes in file_cache.items(): + combined_cache.setdefault(robot, set()).update(nodes) + _save_driver_node_cache(combined_cache) + if robots and workspace_name: + for robot in robots: + _deploy_gui_cache_to_robot(robot, workspace_name) + + if combined_cache: + gui._driver_nodes_by_robot = combined_cache + elif hasattr(gui, "_driver_nodes_by_robot"): + delattr(gui, "_driver_nodes_by_robot") + + robots_needing_fallback = robots + if gui is not None and robots: + sessions = getattr(gui, "_driver_control_sessions", {}) + robots_needing_fallback = [] + for robot in robots: + session = sessions.get(robot) + if session and session.is_alive(): + for cmd in _driver_kill_commands(robot): + session.send(cmd) + session.close() + sessions.pop(robot, None) + else: + robots_needing_fallback.append(robot) + gui._driver_control_sessions = sessions + + if gui is not None and robots_needing_fallback: + _run_remote_commands( + gui, + "Stopping remote driver launch", + [_driver_kill_script], + use_workspace_debug=True, + target_robots=robots_needing_fallback, + ) + + # Best-effort fallback in case processes were started outside this session. + try: + _popen_with_debug("pkill -f 'terminator.*Driver'", gui, shell=True) + except Exception as e: + print(f"Error stopping processes: {e}") + +def move_to_home_pose(gui, UR_prefix): + """Moves the selected robots to the initial pose with the correct namespace and move_group_name.""" + selected_robots = gui.get_selected_robots() + + # Set move_group_name based on UR_prefix + move_group_name = "UR_arm_l" if UR_prefix == "UR10_l" else "UR_arm_r" + + for robot in selected_robots: + # Special case for mur620c with UR10_r + if robot == "mur620c" and UR_prefix == "UR10_r": + home_position = "Home_custom" + elif robot in ["mur620a", "mur620b"]: + home_position = "Home_custom" + else: # Default case for mur620c, mur620d + home_position = "Home_custom" + + # ROS launch command with namespace + command = f"ROS_NAMESPACE={robot} roslaunch ur_utilities move_UR_to_home_pose.launch tf_prefix:={robot} UR_prefix:={UR_prefix} home_position:={home_position} move_group_name:={move_group_name}" + print(f"Executing: {command}") + _popen_with_debug(command, gui, shell=True) + + +def _get_gui_component_name(gui) -> str: + if gui is not None and hasattr(gui, "get_selected_component_name"): + getter = getattr(gui, "get_selected_component_name") + if callable(getter): + try: + name = getter() + if isinstance(name, str) and name.strip(): + return name.strip() + except Exception as exc: + print(f"Failed to fetch component selection from GUI: {exc}") + return DEFAULT_COMPONENT_NAME + + +def _get_gui_component_transform(gui) -> dict: + component_name = _get_gui_component_name(gui) + ros_iface = getattr(gui, "ros_interface", None) + if ros_iface is not None and hasattr(ros_iface, "get_component_transform"): + try: + return ros_iface.get_component_transform(component_name) + except Exception as exc: + print(f"Failed to fetch component transform for {component_name}: {exc}") + return _load_component_transform_entry(component_name) + +def parse_mir_path(gui): + selected_robots = gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping MiR path parsing.") + return + + component_name = _get_gui_component_name(gui) + transform = _get_gui_component_transform(gui) + component_arg = shlex.quote(component_name) + transform_flags = " ".join( + f"{key}:={value}" for key, value in transform.items() + ) + command = f"roslaunch parse_mir_path parse_mir_path.launch component_name:={component_arg} {transform_flags}" + + _run_remote_commands( + gui, + "Parsing MiR path", + [command], + use_workspace_debug=True, + target_robots=selected_robots, + ) + +def parse_ur_path(gui): + selected_robots = gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping UR path parsing.") + return + + component_name = _get_gui_component_name(gui) + transform = _get_gui_component_transform(gui) + component_arg = shlex.quote(component_name) + transform_flags = " ".join( + f"{key}:={value}" for key, value in transform.items() + ) + command = f"roslaunch parse_ur_path parse_ur_path.launch component_name:={component_arg} {transform_flags}" + + _run_remote_commands( + gui, + "Parsing UR path", + [command], + use_workspace_debug=True, + target_robots=selected_robots, + ) + + +def _persist_gui_index_setting(gui): + if gui is None: + return + ros_iface = getattr(gui, "ros_interface", None) + idx_spin = getattr(gui, "idx_spin", None) + if ros_iface is None or idx_spin is None: + return + try: + ros_iface.persist_path_index(idx_spin.value()) + except Exception as exc: + print(f"Failed to persist path index default: {exc}") + + +def move_mir_to_start_pose(gui): + """Moves the MIR robot to the start pose.""" + selected_robots = gui.get_selected_robots() + if selected_robots is None: + print("MIR robot not selected. Skipping move to start pose.") + return + + # Ensure only one MIR robot is selected + if len(selected_robots) != 1: + print("Please select only the MIR robot to move to the start pose.") + return + _persist_gui_index_setting(gui) + command = f"roslaunch move_mir_to_start_pose move_mir_to_start_pose.launch robot_name:={selected_robots[0]} initial_path_index:={gui.idx_spin.value()}" + rospy.loginfo(f"Executing: {command}") + _popen_with_debug(command, gui, shell=True) + +def move_ur_to_start_pose(gui): + """Moves the UR robot to the start pose.""" + selected_robots = gui.get_selected_robots() + selected_urs = gui.get_selected_urs() + + if not selected_robots or not selected_urs: + print("No robots or URs selected. Skipping move to start pose.") + return + + # Ensure only one UR and one mir robot is selected + if len(selected_robots) != 1 or len(selected_urs) != 1: + print("Please select exactly one UR and one MIR robot to move to the start pose.") + return + + # ToDo: to risky to use for now + if selected_urs[0] == "UR10_l": + move_group_name = "UR_arm_l" + planning_group = "UR10_l/tool0" + else: + move_group_name = "UR_arm_r" + planning_group = "UR10_r/tool0" + + spray_distance = gui.get_spray_distance() + + _persist_gui_index_setting(gui) + for robot in selected_robots: + for ur in selected_urs: + command = f"roslaunch move_ur_to_start_pose move_ur_to_start_pose.launch robot_name:={robot} initial_path_index:={gui.idx_spin.value()} spray_distance:={spray_distance}" + print(f"Executing: {command}") + _popen_with_debug(command, gui, shell=True) + +def mir_follow_trajectory(gui): + """Moves the MIR robot along a predefined trajectory.""" + selected_robots = gui.get_selected_robots() + if not selected_robots: + print("No MIR robot selected. Skipping follow trajectory.") + return + # Ensure only one MIR robot is selected + if len(selected_robots) != 1: + print("Please select only the MIR robot to follow the trajectory.") + return + _persist_gui_index_setting(gui) + initial_idx = gui.idx_spin.value() + _run_remote_commands( + gui, + "Launching MiR trajectory follower", + [lambda robot: f"roslaunch mir_trajectory_follower mir_trajectory_follower.launch robot_name:={robot} initial_path_index:={initial_idx}"], + use_workspace_debug=True, + target_robots=selected_robots, + ) + +def increment_path_index(gui): + """Increments the path index for the MIR robot.""" + selected_robots = gui.get_selected_robots() + if not selected_robots: + print("No robot selected. Skipping path index increment.") + return + + initial_idx = gui.idx_spin.value() + _run_remote_commands( + gui, + "Incrementing path index", + [f"roslaunch print_gui increment_path_index.launch initial_path_index:={initial_idx}"], + use_workspace_debug=True, + target_robots=selected_robots, + ) + + # if not rospy.core.is_initialized(): + # rospy.init_node("additive_manufacturing_gui", anonymous=True, disable_signals=True) + # rospy.Subscriber('/path_index', Int32, gui.ros_interface._path_idx_cb, queue_size=10) + + +def stop_mir_motion(gui): + """Stops MiR motion on the selected robots via SSH.""" + _run_remote_commands( + gui, + "Stopping MiR motion", + [ + "pkill -f mir_trajectory_follower || true", + "pkill -f increment_path_index || true", + ], + ) + + +def stop_ur_motion(gui): + """Stops UR motion on the selected robots via SSH.""" + _run_remote_commands( + gui, + "Stopping UR motion", + [ + "pkill -f 'ur_direction_controller|orthogonal_error_correction|move_ur_to_start_pose|ur_vel_induced_by_mir|world_twist_in_mir|twist_combiner|ur_yaw_controller' || true", + ], + ) + + +def stop_all_but_drivers(gui): + """Stops common non-driver processes on the selected robots via SSH.""" + if gui is None: + return + + selected_robots = gui.get_selected_robots() + if not selected_robots: + print("No robots selected. Skipping non-driver stop.") + return + + workspace = gui.get_workspace_name() + commands = _stop_non_driver_commands() + sessions = getattr(gui, "_driver_control_sessions", {}) + robots_needing_fallback = [] + + for robot in selected_robots: + session = sessions.get(robot) + if session is None or not session.is_alive(): + session = _ensure_driver_control_session(gui, robot, workspace) + if session and session.is_alive(): + for cmd in commands: + session.send(cmd) + else: + robots_needing_fallback.append(robot) + + gui._driver_control_sessions = sessions + + if robots_needing_fallback: + _run_remote_commands( + gui, + "Stopping non-driver processes", + [_stop_non_driver_script], + use_workspace_debug=True, + target_robots=robots_needing_fallback, + ) + +def ur_follow_trajectory(gui, ur_follow_settings: dict): + """Moves the UR robot along a predefined trajectory.""" + selected_robots = gui.get_selected_robots() + selected_urs = gui.get_selected_urs() + metric = ur_follow_settings.get("idx_metric") + threshold = ur_follow_settings.get("threshold") + initial_path_index = gui.idx_spin.value() + spray_distance = gui.get_spray_distance() + rospy.loginfo(f"Selected metric: {metric}") + + if not selected_robots or not selected_urs: + print("No UR robot selected. Skipping follow trajectory.") + return + + # Ensure only one UR and one MIR robot is selected + if len(selected_robots) != 1 or len(selected_urs) != 1: + print("Please select exactly one UR and one MIR robot to follow the trajectory.") + return + + ur = selected_urs[0] + + cmd_template = ( + "roslaunch print_hw complete_ur_trajectory_follower_ff_only.launch " + "robot_name:={robot} prefix_ur:={ur}/ metric:='{metric}' " + "threshold:={threshold} initial_path_index:={initial_path_index} " + "nozzle_height_default:={spray_distance}" + ) + + _run_remote_commands( + gui, + "Launching UR trajectory follower", + [ + lambda robot, template=cmd_template: template.format( + robot=robot, + ur=ur, + metric=metric, + threshold=threshold, + initial_path_index=initial_path_index, + spray_distance=spray_distance, + ) + ], + use_workspace_debug=True, + target_robots=selected_robots, + ) + +def stop_idx_advancer(gui): + """Stops the path index advancer nodes on the selected robots via SSH.""" + _run_remote_commands( + gui, + "Stopping path index advancer", + [ + "pkill -f path_index_advancer || true", + "pkill -f increment_path_index || true", + ], + ) + + +def target_broadcaster(gui): + """Broadcasts Target Poses.""" + command = f"roslaunch print_hw target_broadcaster.launch initial_path_index:={gui.idx_spin.value()}" + print(f"Executing: {command}") + _popen_with_debug(command, gui, shell=True) \ No newline at end of file diff --git a/general_mur_gui/scripts/ros_node_monitor_ur.py b/general_mur_gui/scripts/ros_node_monitor_ur.py new file mode 100644 index 00000000..87043121 --- /dev/null +++ b/general_mur_gui/scripts/ros_node_monitor_ur.py @@ -0,0 +1,108 @@ +import sys +import rospy +import rosgraph +from PyQt5 import QtWidgets, QtCore + +class RosNodeMonitor(QtWidgets.QWidget): + REFRESH_MS = 2000 # how often to poll [ms] + + def __init__(self, watch_nodes): + """ + :param watch_nodes: list of node names to monitor, e.g. ['/twist_combiner_world', ...] + """ + super().__init__() + rospy.init_node('ros_node_monitor_gui', anonymous=True) + self.master = rosgraph.masterapi.Master('/ros_node_monitor_gui') + self.watch_nodes = watch_nodes + + # Set up table: columns: Node, Running?, Publishers, Subscribers + self.table = QtWidgets.QTableWidget(len(watch_nodes), 4) + self.table.setHorizontalHeaderLabels(['Node', 'Up?', 'Publishes', 'Subscribes']) + for row, nm in enumerate(watch_nodes): + self.table.setItem(row, 0, QtWidgets.QTableWidgetItem(nm)) + self.table.verticalHeader().setVisible(False) + self.table.horizontalHeader().setStretchLastSection(True) + + layout = QtWidgets.QVBoxLayout() + layout.addWidget(self.table) + self.setLayout(layout) + + # Poll timer + self.timer = QtCore.QTimer(self) + self.timer.timeout.connect(self.update_status) + self.timer.start(self.REFRESH_MS) + self.update_status() + + def update_status(self): + # getSystemState() → (publishers, subscribers, services) + try: + pubs, subs, services = self.master.getSystemState() + except Exception as e: + rospy.logwarn(f"Could not contact master: {e}") + return + + # build topic→nodes mapping + topic_pubs = { t: nodes for t, nodes in pubs } + topic_subs = { t: nodes for t, nodes in subs } + + # build node→[topics] maps + node_to_pubs = { n: [] for n in self.watch_nodes } + node_to_subs = { n: [] for n in self.watch_nodes } + + # For each topic, for each node in publisher list, record it + for topic, nodes in topic_pubs.items(): + for n in nodes: + if n in node_to_pubs: + node_to_pubs[n].append(topic) + for topic, nodes in topic_subs.items(): + for n in nodes: + if n in node_to_subs: + node_to_subs[n].append(topic) + + # also pull list of all active nodes + try: + # active = set(self.master.getSystemState()[2]) # outdated; better: use getPid or rosnode API + # # Actually, simple alternative: rosnode list + # code2, msg2, node_list = self.master.getSystemState() + # But getSystemState only lists topics; instead: + node_list = set() + for t,n_list in pubs + subs: + node_list.update(n_list) + except: + node_list = set() + + # Update table rows + for row, nm in enumerate(self.watch_nodes): + is_up = nm in node_list + self.table.setItem(row, 1, QtWidgets.QTableWidgetItem("✔" if is_up else "✘")) + self.table.setItem(row, 2, QtWidgets.QTableWidgetItem("\n".join(node_to_pubs.get(nm, [])))) + self.table.setItem(row, 3, QtWidgets.QTableWidgetItem("\n".join(node_to_subs.get(nm, [])))) + + # color the “Up?” cell + item = self.table.item(row, 1) + if is_up: + item.setBackground(QtCore.Qt.green) + else: + item.setBackground(QtCore.Qt.red) + +if __name__ == '__main__': + # list the exact names from your launch file: + watch = [ + '/path_index_advancer', + '/twist_combiner_world', + '/twist_combiner_profile_offset', + '/tcp_in_base_ideal_publisher', + '/twist_combiner', + '/ur_vel_induced_by_mir', + '/world_twist_in_mir', + '/ur_direction_controller', + '/pid_twist_controller_direction/pid_twist_controller_direction', + '/pid_twist_controller_orthogonal/pid_twist_controller_orthogonal', + + ] + app = QtWidgets.QApplication(sys.argv) + w = RosNodeMonitor(watch) + w.resize(800, 300) + w.setWindowTitle("ROS Node Monitor") + w.show() + sys.exit(app.exec_()) diff --git a/general_mur_gui/scripts/turn_on_all_arm_controllers.py b/general_mur_gui/scripts/turn_on_all_arm_controllers.py new file mode 100755 index 00000000..e3c5640d --- /dev/null +++ b/general_mur_gui/scripts/turn_on_all_arm_controllers.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 + +# first load all twist controllers +# call the switch controller service for all robots + +import rospy +from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest, LoadController, LoadControllerRequest + +class TurnOnAllTwistControllers: + + def config(self): + self.robot_names = rospy.get_param('~robot_names', ['mur620a','mur620b','mur620c', 'mur620d']) + self.UR_prefixes = rospy.get_param('~UR_prefixes', ['UR10_l', 'UR10_r']) + self.controller_name = rospy.get_param('~controller_name', 'arm_controller') + self.old_controller_name = rospy.get_param('~old_controller_name', 'twist_controller') + + def __init__(self): + self.config() + self.turn_on_twist_controllers() + + + def turn_on_twist_controllers(self): + for robot_name in self.robot_names: + for UR_prefix in self.UR_prefixes: + load_controller_client = rospy.ServiceProxy(robot_name + "/" + UR_prefix + "/controller_manager/load_controller", LoadController) + rospy.loginfo("Waiting for " + robot_name + "/" + UR_prefix + "/controller_manager/load_controller") + load_controller_client.wait_for_service() + rospy.loginfo("Loading " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + load_controller_request = LoadControllerRequest() + load_controller_request.name = self.controller_name + load_controller_client(load_controller_request) + rospy.loginfo("Loaded " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + + + switch_controller_client = rospy.ServiceProxy(robot_name + "/" + UR_prefix + "/controller_manager/switch_controller", SwitchController) + rospy.loginfo("Waiting for " + robot_name + "/" + UR_prefix + "/controller_manager/switch_controller") + switch_controller_client.wait_for_service() + rospy.loginfo("Switching to " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + switch_controller_request = SwitchControllerRequest() + switch_controller_request.start_controllers = [self.controller_name] + switch_controller_request.stop_controllers = [self.old_controller_name] + switch_controller_request.strictness = 1 + switch_controller_client(switch_controller_request) + rospy.loginfo("Switched to " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + + + +if __name__ == '__main__': + rospy.init_node('turn_on_all_twist_controllers') + TurnOnAllTwistControllers() + rospy.spin() \ No newline at end of file diff --git a/general_mur_gui/scripts/turn_on_all_twist_controllers.py b/general_mur_gui/scripts/turn_on_all_twist_controllers.py new file mode 100755 index 00000000..2f694bfa --- /dev/null +++ b/general_mur_gui/scripts/turn_on_all_twist_controllers.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 + +# first load all twist controllers +# call the switch controller service for all robots + +import rospy +from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest, LoadController, LoadControllerRequest + +class TurnOnAllTwistControllers: + + def config(self): + self.robot_names = rospy.get_param('~robot_names', ['mur620a','mur620b','mur620c', 'mur620d']) + self.UR_prefixes = rospy.get_param('~UR_prefixes', ['UR10_l', 'UR10_r']) + self.controller_name = rospy.get_param('~controller_name', 'twist_controller') + self.old_controller_name = rospy.get_param('~old_controller_name', 'arm_controller') + + def __init__(self): + self.config() + self.turn_on_twist_controllers() + + + def turn_on_twist_controllers(self): + for robot_name in self.robot_names: + for UR_prefix in self.UR_prefixes: + load_controller_client = rospy.ServiceProxy(robot_name + "/" + UR_prefix + "/controller_manager/load_controller", LoadController) + rospy.loginfo("Waiting for " + robot_name + "/" + UR_prefix + "/controller_manager/load_controller") + load_controller_client.wait_for_service() + rospy.loginfo("Loading " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + load_controller_request = LoadControllerRequest() + load_controller_request.name = self.controller_name + load_controller_client(load_controller_request) + rospy.loginfo("Loaded " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + + + switch_controller_client = rospy.ServiceProxy(robot_name + "/" + UR_prefix + "/controller_manager/switch_controller", SwitchController) + rospy.loginfo("Waiting for " + robot_name + "/" + UR_prefix + "/controller_manager/switch_controller") + switch_controller_client.wait_for_service() + rospy.loginfo("Switching to " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + switch_controller_request = SwitchControllerRequest() + switch_controller_request.start_controllers = [self.controller_name] + switch_controller_request.stop_controllers = [self.old_controller_name] + switch_controller_request.strictness = 1 + switch_controller_client(switch_controller_request) + rospy.loginfo("Switched to " + self.controller_name + " for " + robot_name + "/" + UR_prefix) + + + +if __name__ == '__main__': + rospy.init_node('turn_on_all_twist_controllers') + TurnOnAllTwistControllers() + rospy.spin() \ No newline at end of file diff --git a/match_gazebo/include/gazebo_ros_link_attacher.h b/match_gazebo/include/gazebo_ros_link_attacher.h deleted file mode 100644 index f01feb19..00000000 --- a/match_gazebo/include/gazebo_ros_link_attacher.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Desc: Gazebo link attacher plugin. - * Author: Sammy Pfeiffer (sam.pfeiffer@pal-robotics.com) - * Date: 05/04/2016 - */ - -#ifndef GAZEBO_ROS_LINK_ATTACHER_HH -#define GAZEBO_ROS_LINK_ATTACHER_HH - -#include - -#include - -#include -#include "gazebo/gazebo.hh" -#include -#include "gazebo/physics/PhysicsTypes.hh" -#include -#include -#include -#include -#include "gazebo/msgs/msgs.hh" -#include "gazebo/transport/transport.hh" -#include "gazebo_ros_link_attacher/Attach.h" -#include "gazebo_ros_link_attacher/AttachRequest.h" -#include "gazebo_ros_link_attacher/AttachResponse.h" - -namespace gazebo -{ - - class GazeboRosLinkAttacher : public WorldPlugin - { - public: - /// \brief Constructor - GazeboRosLinkAttacher(); - - /// \brief Destructor - virtual ~GazeboRosLinkAttacher(); - - /// \brief Load the controller - void Load( physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/ ); - - /// \brief Attach with a revolute joint - bool attach(std::string model1, std::string link1, - std::string model2, std::string link2); - - /// \brief Detach - bool detach(std::string model1, std::string link1, - std::string model2, std::string link2); - - /// \brief Internal representation of a fixed joint - struct fixedJoint{ - std::string model1; - physics::ModelPtr m1; - std::string link1; - physics::LinkPtr l1; - std::string model2; - physics::ModelPtr m2; - std::string link2; - physics::LinkPtr l2; - physics::JointPtr joint; - }; - - bool getJoint(std::string model1, std::string link1, std::string model2, std::string link2, fixedJoint &joint); - - private: - ros::NodeHandle nh_; - ros::ServiceServer attach_service_; - ros::ServiceServer detach_service_; - - bool attach_callback(gazebo_ros_link_attacher::Attach::Request &req, - gazebo_ros_link_attacher::Attach::Response &res); - bool detach_callback(gazebo_ros_link_attacher::Attach::Request &req, - gazebo_ros_link_attacher::Attach::Response &res); - - std::vector joints; - - boost::recursive_mutex* physics_mutex; - - /// \brief The physics engine. - physics::PhysicsEnginePtr physics; - - /// \brief Pointer to the world. - physics::WorldPtr world; - - }; - -} - -#endif - diff --git a/mir/mir_description/urdf/mir_600/mir_600_top_module.urdf.xacro b/mir/mir_description/urdf/mir_600/mir_600_top_module.urdf.xacro index 3a8d871a..036ee54e 100644 --- a/mir/mir_description/urdf/mir_600/mir_600_top_module.urdf.xacro +++ b/mir/mir_description/urdf/mir_600/mir_600_top_module.urdf.xacro @@ -89,7 +89,7 @@ - + diff --git a/mir/mir_launch_hardware/launch/general_mir.launch b/mir/mir_launch_hardware/launch/general_mir.launch index a76b5341..51687e9a 100644 --- a/mir/mir_launch_hardware/launch/general_mir.launch +++ b/mir/mir_launch_hardware/launch/general_mir.launch @@ -13,7 +13,7 @@ - + @@ -49,7 +49,7 @@ - + diff --git a/mir/mir_launch_hardware/launch/mir_600.launch b/mir/mir_launch_hardware/launch/mir_600.launch index e6c800e9..f4b02426 100644 --- a/mir/mir_launch_hardware/launch/mir_600.launch +++ b/mir/mir_launch_hardware/launch/mir_600.launch @@ -9,7 +9,7 @@ - + diff --git a/mir/mir_launch_hardware/scripts/external_localization_broadcaster.py b/mir/mir_launch_hardware/scripts/external_localization_broadcaster.py index 6eb0f818..bc0ea7ca 100755 --- a/mir/mir_launch_hardware/scripts/external_localization_broadcaster.py +++ b/mir/mir_launch_hardware/scripts/external_localization_broadcaster.py @@ -10,7 +10,7 @@ def __init__(self): self.tf_prefix = rospy.get_param('~tf_prefix', 'mur620d') self.pose_broadcaster = tf.TransformBroadcaster() self.localization_topic = rospy.get_param('~localization_topic', '/qualisys/mur620d/pose') - self.mocap_offset = rospy.get_param('~mocap_offset', [38.2691, 32.8942, 3.1656]) + self.mocap_offset = rospy.get_param('~mocap_offset', [0.0, 0.0, 0.0]) self.sub = rospy.Subscriber(self.localization_topic, PoseStamped, self.callback) self.timestamp = rospy.Time.now() rospy.spin() diff --git a/mir/mir_navigation/nodes/mir_pose_simple.py b/mir/mir_navigation/nodes/mir_pose_simple.py index 5df41833..0bfca6d5 100755 --- a/mir/mir_navigation/nodes/mir_pose_simple.py +++ b/mir/mir_navigation/nodes/mir_pose_simple.py @@ -9,7 +9,7 @@ class Mir_pose_simple(): def __init__(self): # Get params - self.localization_type = rospy.get_param("~localization_type", "amcl") + self.localization_type = rospy.get_param("~localization_type", "robot_pose") self.odom_topic = rospy.get_param("~odom_topic", "odom") self.amcl_pose_topic = rospy.get_param("~amcl_pose_topic", "amcl_pose") self.groud_truth_topic = rospy.get_param("~groud_truth_topic", "groud_truth") diff --git a/mir/mir_submodules/ewellix_tlt b/mir/mir_submodules/ewellix_tlt index da3a1e0b..e5bbe2e0 160000 --- a/mir/mir_submodules/ewellix_tlt +++ b/mir/mir_submodules/ewellix_tlt @@ -1 +1 @@ -Subproject commit da3a1e0bc74eb44fc1d56c7d9408a2222c4ea1fc +Subproject commit e5bbe2e060ccb525226f91c467609184dae90a85 diff --git a/mir/mir_submodules/mir_robot b/mir/mir_submodules/mir_robot index 966b9fad..f60ab172 160000 --- a/mir/mir_submodules/mir_robot +++ b/mir/mir_submodules/mir_robot @@ -1 +1 @@ -Subproject commit 966b9fad85ad055d239d0c909f40704fbb5a540e +Subproject commit f60ab172292bdd53b27307faa9478b41e338a1e7 diff --git a/mur/mur_control/launch/global_tcp_pose_publisher.launch b/mur/mur_control/launch/global_tcp_pose_publisher.launch index cffa054b..4fc8c2a9 100644 --- a/mur/mur_control/launch/global_tcp_pose_publisher.launch +++ b/mur/mur_control/launch/global_tcp_pose_publisher.launch @@ -6,17 +6,30 @@ + + + + + + + - + - + + + + + + + \ No newline at end of file diff --git a/mur/mur_control/scripts/pose_composer.py b/mur/mur_control/scripts/pose_composer.py index a032e6bf..248a6685 100755 --- a/mur/mur_control/scripts/pose_composer.py +++ b/mur/mur_control/scripts/pose_composer.py @@ -2,6 +2,7 @@ import rospy from geometry_msgs.msg import Pose, PoseStamped +from sensor_msgs.msg import JointState from tf import TransformListener from tf.transformations import quaternion_matrix, quaternion_from_matrix, concatenate_matrices @@ -29,6 +30,7 @@ class TotalPose: def __init__(self): # msg_class_base,_,_=rostopic.get_topic_class("world_T_base", blocking=True) self.world_T_base = np.eye(4) + self.base_T_ee_raw = np.eye(4) self.base_T_ee = np.eye(4) self.world_T_ee = PoseStamped() self.world_T_ee.header.frame_id = rospy.get_param("~world_frame", "map") @@ -37,24 +39,90 @@ def __init__(self): self.static_tf_included = rospy.get_param("~static_tf_included", False) static_tf_parent = rospy.get_param("~static_tf_parent", "mur620/base_footprint") static_tf_child = rospy.get_param("~static_tf_child", "mur620/UR10_l/base_link") + self.mocap_mode = rospy.get_param("~mocap_mode", False) self.static_T = None + self.tf_listener = TransformListener() + + self.static_T = np.eye(4) + self.lift_pre_T = np.eye(4) + self.lift_post_T = np.eye(4) + self.dynamic_lift_T = np.eye(4) + self.lift_transforms_ready = False + + self.lift_enabled = rospy.get_param("~lift_enabled", False) + self.lift_joint_name = rospy.get_param("~lift_joint_name", "") + self.lift_joint_states_topic = rospy.get_param("~lift_joint_states_topic", "/joint_states") + self.lift_bottom_frame = rospy.get_param("~lift_bottom_frame", "") + self.lift_top_frame = rospy.get_param("~lift_top_frame", "") + axis = rospy.get_param("~lift_axis", "z").lower() + axis_map = {"x": 0, "y": 1, "z": 2} + if axis not in axis_map: + rospy.logwarn(f"Invalid lift axis '{axis}'. Falling back to 'z'.") + axis = "z" + self.lift_axis_index = axis_map[axis] + self.lift_joint_seen = False + + if self.lift_enabled: + missing = [] + if not self.lift_joint_name: + missing.append("lift_joint_name") + if not self.lift_bottom_frame: + missing.append("lift_bottom_frame") + if not self.lift_top_frame: + missing.append("lift_top_frame") + if not self.lift_joint_states_topic: + missing.append("lift_joint_states_topic") + if missing: + rospy.logwarn( + "Lift integration requested but missing parameters: %s. Disabling lift integration." % ", ".join(missing) + ) + self.lift_enabled = False + if self.static_tf_included: - tf_listener = TransformListener() - - while not rospy.is_shutdown(): - try: - tf_listener.waitForTransform(static_tf_parent, static_tf_child, rospy.Time(0), rospy.Duration(5.0)) - static_trans, static_rot = tf_listener.lookupTransform(static_tf_parent, static_tf_child, rospy.Time(0)) - break - except Exception as e: - rospy.logwarn(f"Could not get static transform: {e}. Retrying...") - rospy.sleep(1.0) - self.static_T = quaternion_matrix([static_rot[0], static_rot[1], static_rot[2], static_rot[3]]) - self.static_T[0:3, 3] = static_trans + if self.lift_enabled: + self.lift_pre_T = self._lookup_transform(static_tf_parent, self.lift_bottom_frame) + self.lift_post_T = self._lookup_transform(self.lift_top_frame, static_tf_child) + self.lift_transforms_ready = True + else: + self.static_T = self._lookup_transform(static_tf_parent, static_tf_child) + elif self.lift_enabled: + rospy.logwarn("Lift integration requires static transforms. Disabling lift.") + self.lift_enabled = False + self.world_T_ee_pub = rospy.Publisher("world_T_ee", PoseStamped, queue_size=1) + rospy.sleep(1.0) # Allow publisher to set up self.world_T_base_sub = rospy.Subscriber("world_T_base", PoseStamped, self.world_T_base_callback) self.base_T_ee_sub = rospy.Subscriber("base_T_ee", PoseStamped, self.base_T_ee_callback) - self.world_T_ee_pub = rospy.Publisher("world_T_ee", PoseStamped, queue_size=1) + + self.lift_joint_sub = None + if self.lift_enabled: + self.lift_joint_sub = rospy.Subscriber( + self.lift_joint_states_topic, JointState, self.lift_joint_callback, queue_size=1 + ) + + def _lookup_transform(self, parent: str, child: str) -> np.ndarray: + if not parent or not child: + rospy.logwarn("Cannot lookup transform with empty frame ids. Using identity transform.") + return np.eye(4) + + while not rospy.is_shutdown(): + try: + self.tf_listener.waitForTransform(parent, child, rospy.Time(0), rospy.Duration(5.0)) + trans, rot = self.tf_listener.lookupTransform(parent, child, rospy.Time(0)) + matrix = quaternion_matrix([rot[0], rot[1], rot[2], rot[3]]) + matrix[0:3, 3] = trans + return matrix + except Exception as e: + rospy.logwarn(f"Could not get transform from {parent} to {child}: {e}. Retrying...") + rospy.sleep(1.0) + return np.eye(4) + + def _apply_static_chain(self, base_T_ee: np.ndarray) -> np.ndarray: + if not self.static_tf_included: + return base_T_ee + if self.lift_enabled and self.lift_transforms_ready: + return concatenate_matrices(self.lift_pre_T, self.dynamic_lift_T, self.lift_post_T, base_T_ee) + return concatenate_matrices(self.static_T, base_T_ee) def world_T_base_callback(self, msg: PoseStamped): self.world_T_base = pose_to_matrix(msg.pose) @@ -63,11 +131,31 @@ def world_T_base_callback(self, msg: PoseStamped): def base_T_ee_callback(self, msg: PoseStamped): base_T_ee = pose_to_matrix(msg.pose) - if self.static_tf_included: - base_T_ee = concatenate_matrices(self.static_T, base_T_ee) - self.base_T_ee = base_T_ee + self.base_T_ee_raw = base_T_ee + self.base_T_ee = self._apply_static_chain(base_T_ee) self.world_T_ee.header.stamp = msg.header.stamp + if not self.mocap_mode: + self.update_world_T_ee() + + def lift_joint_callback(self, msg: JointState): + try: + lift_index = msg.name.index(self.lift_joint_name) + self.lift_joint_seen = True + except ValueError: + if not self.lift_joint_seen: + rospy.logwarn_throttle(5.0, f"Lift joint '{self.lift_joint_name}' not found in JointState message. Waiting...") + return + + if lift_index >= len(msg.position): + return + + translation_value = msg.position[lift_index] + translation = [0.0, 0.0, 0.0] + translation[self.lift_axis_index] = translation_value + self.dynamic_lift_T[0:3, 3] = translation + + self.base_T_ee = self._apply_static_chain(self.base_T_ee_raw) self.update_world_T_ee() def update_world_T_ee(self): diff --git a/mur/mur_launch_hardware/launch/general_mur600.launch b/mur/mur_launch_hardware/launch/general_mur600.launch index 8af10730..c190b98b 100644 --- a/mur/mur_launch_hardware/launch/general_mur600.launch +++ b/mur/mur_launch_hardware/launch/general_mur600.launch @@ -25,6 +25,8 @@ + + @@ -68,7 +70,7 @@ - + @@ -104,8 +106,9 @@ + - + @@ -117,18 +120,25 @@ - + - + + - - --> + + + + + + + + @@ -138,7 +148,7 @@ - + @@ -158,6 +168,7 @@ + @@ -166,16 +177,36 @@ - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + @@ -192,6 +223,16 @@ + + + + + + + + + + - + @@ -9,9 +9,13 @@ - - + + + + + + @@ -40,6 +44,8 @@ + + diff --git a/mur/mur_launch_hardware/launch/mur620d.launch b/mur/mur_launch_hardware/launch/mur620d.launch index f6fc05c3..b294da35 100644 --- a/mur/mur_launch_hardware/launch/mur620d.launch +++ b/mur/mur_launch_hardware/launch/mur620d.launch @@ -9,8 +9,9 @@ - - + + + @@ -30,13 +31,15 @@ - - + + + + diff --git a/mur/mur_launch_hardware/launch/mur_base.launch b/mur/mur_launch_hardware/launch/mur_base.launch index 067f4283..b94d7639 100644 --- a/mur/mur_launch_hardware/launch/mur_base.launch +++ b/mur/mur_launch_hardware/launch/mur_base.launch @@ -13,7 +13,7 @@ - + diff --git a/mur/mur_launch_hardware/scripts/joint_state_aggregator.py b/mur/mur_launch_hardware/scripts/joint_state_aggregator.py new file mode 100755 index 00000000..6b0fae25 --- /dev/null +++ b/mur/mur_launch_hardware/scripts/joint_state_aggregator.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +import rospy +from sensor_msgs.msg import JointState +from threading import Lock + +class JointStateAggregator: + def __init__(self): + self.lock = Lock() + self.state = {} # name -> (pos, vel, eff) + self.have_vel = False + self.have_eff = False + + # Params + self.topic_mir = rospy.get_param("~topic_mir", "") + self.topic_ur_l = rospy.get_param("~topic_ur_l", "") + self.topic_ur_r = rospy.get_param("~topic_ur_r", "") + self.topic_lift_l = rospy.get_param("~topic_lift_l", "") + self.topic_lift_r = rospy.get_param("~topic_lift_r", "") + self.out_topic = rospy.get_param("~out_topic", "joint_states") + self.frame_id = rospy.get_param("~frame_id", "") + + # Publisher + self.pub = rospy.Publisher(self.out_topic, JointState, queue_size=10) + + # Subscribers (only if topic is set) + if self.topic_mir: + rospy.Subscriber(self.topic_mir, JointState, self._cb_cache, queue_size=5, tcp_nodelay=True) + if self.topic_lift_l: + rospy.Subscriber(self.topic_lift_l, JointState, self._cb_cache, queue_size=5, tcp_nodelay=True) + if self.topic_lift_r: + rospy.Subscriber(self.topic_lift_r, JointState, self._cb_cache, queue_size=5, tcp_nodelay=True) + if self.topic_ur_l: + rospy.Subscriber(self.topic_ur_l, JointState, self._cb_cache, queue_size=5, tcp_nodelay=True) + self.publish_rate_hz = rospy.get_param("~publish_rate_hz", 500.0) + if self.topic_ur_r: + self.publish_rate_hz = rospy.get_param("~publish_rate_hz", 500.0) + rospy.Subscriber(self.topic_ur_r, JointState, self._cb_cache, queue_size=5, tcp_nodelay=True) + if not self.topic_ur_l and not self.topic_ur_r: + self.publish_rate_hz = 100.0 + rospy.logwarn("~topic_ur is empty -> publishing periodically at %.1f Hz", self.publish_rate_hz) + + self.timer = rospy.Timer(rospy.Duration(1.0 / self.publish_rate_hz), self._timer_publish) + rospy.loginfo("JointStateAggregator out=%s", self.out_topic) + rospy.loginfo(" mir: %s", self.topic_mir) + rospy.loginfo(" ur: %s", self.topic_ur_l) + rospy.loginfo(" lift: %s", self.topic_lift_l) + + def _cb_cache(self, msg: JointState): + # Cache only + with self.lock: + self._merge(msg) + + def _merge(self, msg: JointState): + n = len(msg.name) + pos = msg.position if len(msg.position) == n else None + vel = msg.velocity if len(msg.velocity) == n else None + eff = msg.effort if len(msg.effort) == n else None + + if vel is not None: + self.have_vel = True + if eff is not None: + self.have_eff = True + + for i, name in enumerate(msg.name): + p = pos[i] if pos is not None else None + v = vel[i] if vel is not None else None + e = eff[i] if eff is not None else None + + old = self.state.get(name, (None, None, None)) + # Update only fields we have in this message; keep previous otherwise + self.state[name] = ( + p if p is not None else old[0], + v if v is not None else old[1], + e if e is not None else old[2], + ) + + def _build_output(self, stamp): + out = JointState() + out.header.stamp = stamp if stamp and stamp != rospy.Time(0) else rospy.Time.now() + if self.frame_id: + out.header.frame_id = self.frame_id + + names = sorted(self.state.keys()) + out.name = names + + # Always fill position; fill vel/eff only if we ever saw consistent arrays + out.position = [self.state[n][0] if self.state[n][0] is not None else 0.0 for n in names] + if self.have_vel: + out.velocity = [self.state[n][1] if self.state[n][1] is not None else 0.0 for n in names] + if self.have_eff: + out.effort = [self.state[n][2] if self.state[n][2] is not None else 0.0 for n in names] + return out + + def _timer_publish(self, _event): + with self.lock: + out = self._build_output(stamp=rospy.Time.now()) + self.pub.publish(out) + + +if __name__ == "__main__": + rospy.init_node("joint_state_aggregator") + JointStateAggregator() + rospy.spin() diff --git a/setup_full.sh b/setup_full.sh index b5c08f75..ba0a26b8 100755 --- a/setup_full.sh +++ b/setup_full.sh @@ -9,26 +9,34 @@ cd build cmake .. make sudo make install -cd ../../../../../.. +cd ../../../../../../.. rosdep update rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y catkin build source devel/setup.bash # install dependencies manually (this should usually be done through rosdep) -sudo apt install ros-noetic-costmap-2d -sudo apt install ros-noetic-serial -sudo apt install ros-noetic-nav-core -sudo apt install ros-noetic-moveit-core -sudo apt install ros-noetic-ur-client-library -sudo apt install ros-noetic-moveit-ros-planning-interface -sudo apt install ros-noetic-mbf-msgs -sudo apt install ros-noetic-mir-actions -sudo apt install ros-noetic-navfn -sudo apt install ros-noetic-industrial-robot-status-interface -sudo apt install ros-noetic-move-base-msgs -sudo apt install ros-noetic-scaled-joint-trajectory-controller -sudo apt install ros-noetic-rospy-message-converter -sudo apt install ros-noetic-speed-scaling-interface -sudo apt install ros-noetic-speed-scaling-state-controller -sudo apt install ros-noetic-pass-through-controllers \ No newline at end of file +sudo apt install ros-one-costmap-2d -y +sudo apt install ros-one-serial -y +sudo apt install ros-one-nav-core -y +sudo apt install ros-one-moveit-core -y +sudo apt install ros-one-ur-client-library -y +sudo apt install ros-one-moveit-ros-planning-interface -y +sudo apt install ros-one-mbf-msgs -y +sudo apt install ros-one-mir-actions -y +sudo apt install ros-one-navfn -y +sudo apt install ros-one-industrial-robot-status-interface -y +sudo apt install ros-one-move-base-msgs -y +sudo apt install ros-one-scaled-joint-trajectory-controller -y +sudo apt install ros-one-rospy-message-converter -y +sudo apt install ros-one-speed-scaling-interface -y +sudo apt install ros-one-speed-scaling-state-controller -y +sudo apt install ros-one-pass-through-controllers -y +sudo apt install libserial-dev -y +sudo apt install ros-one-rqt-* -y +sudo apt install ros-one-moveit-planners* -y +sudo apt install ros-one-moveit-ros* -y +apt install ros-one-ros-control* -y +sudo apt install ros-one-moveit-commander -y +sudo apt install ros-one-pcl-ros -y +sudo apt install ros-one-tf2-sensor-msgs -y diff --git a/submodules/gazebo_ros_link_attacher b/submodules/gazebo_ros_link_attacher deleted file mode 160000 index 1d170c44..00000000 --- a/submodules/gazebo_ros_link_attacher +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 1d170c44c2f12dad4e8c23e299d46f3f655e7def diff --git a/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro b/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro index 725668e2..88ed4a2f 100644 --- a/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro +++ b/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro @@ -11,7 +11,7 @@ - + diff --git a/ur/robotiq b/ur/robotiq deleted file mode 160000 index eaa20fd7..00000000 --- a/ur/robotiq +++ /dev/null @@ -1 +0,0 @@ -Subproject commit eaa20fd75d29c9d122701462b68f0cac4937c45b diff --git a/ur/ur_calibrated_pose_pub/include/ur_calibrated_pose_pub/ur_calibrated_pose_pub.h b/ur/ur_calibrated_pose_pub/include/ur_calibrated_pose_pub/ur_calibrated_pose_pub.h index 323135f0..f90c77d5 100644 --- a/ur/ur_calibrated_pose_pub/include/ur_calibrated_pose_pub/ur_calibrated_pose_pub.h +++ b/ur/ur_calibrated_pose_pub/include/ur_calibrated_pose_pub/ur_calibrated_pose_pub.h @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -43,6 +44,8 @@ namespace ur_calibrated_pose_pub std::string ur_joint_state_topic_name_; std::string joint_prefix_; std::string dh_parameter_switch_; + std::string base_frame_id_; + Eigen::Matrix4d tcp_offset_transform_; std::vector ideal_dh_transformations_list_; std::vector calibrated_dh_transformations_list_; @@ -52,6 +55,9 @@ namespace ur_calibrated_pose_pub // Callback functions void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state_msg); + + std::string sanitizeFrameId(const std::string& frame_id) const; + Eigen::Matrix4d buildTransformFromOffset(const std::vector& offset_vector) const; }; } #endif // ifndef UR_CALIBRATED_POSE_PUB_H_INCLUDED \ No newline at end of file diff --git a/ur/ur_calibrated_pose_pub/launch/ur_calibrated_pose_pub.launch b/ur/ur_calibrated_pose_pub/launch/ur_calibrated_pose_pub.launch index 1b7510cd..95b4e7e6 100644 --- a/ur/ur_calibrated_pose_pub/launch/ur_calibrated_pose_pub.launch +++ b/ur/ur_calibrated_pose_pub/launch/ur_calibrated_pose_pub.launch @@ -6,6 +6,8 @@ ideal: Uses the ideal CAD parameter from the config file --> + + @@ -14,5 +16,7 @@ + + $(arg tcp_offset) \ No newline at end of file diff --git a/ur/ur_calibrated_pose_pub/src/ur_calibrated_pose_pub.cpp b/ur/ur_calibrated_pose_pub/src/ur_calibrated_pose_pub.cpp index 6309e19d..f917d792 100644 --- a/ur/ur_calibrated_pose_pub/src/ur_calibrated_pose_pub.cpp +++ b/ur/ur_calibrated_pose_pub/src/ur_calibrated_pose_pub.cpp @@ -3,7 +3,7 @@ namespace ur_calibrated_pose_pub { URCalibratedPosePub::URCalibratedPosePub(ros::NodeHandle nh, - ros::NodeHandle private_nh) : nh_(nh), private_nh_(private_nh) + ros::NodeHandle private_nh) : nh_(nh), private_nh_(private_nh), tcp_offset_transform_(Eigen::Matrix4d::Identity()) { } void URCalibratedPosePub::init() @@ -21,7 +21,12 @@ namespace ur_calibrated_pose_pub void URCalibratedPosePub::execute() { - ros::Rate publish_rate = ros::Rate(100.0); + const std::string sanitized_namespace = this->sanitizeFrameId(ros::this_node::getNamespace()); + const std::string default_base_frame_id = sanitized_namespace.empty() ? "base" : sanitized_namespace + "/base"; + const std::string parent_frame_id = this->base_frame_id_.empty() ? default_base_frame_id : this->base_frame_id_; + const std::string child_frame_id = sanitized_namespace.empty() ? "calibrated_ee_pose" : sanitized_namespace + "/calibrated_ee_pose"; + + ros::Rate publish_rate = ros::Rate(500.0); while(ros::ok()) { // Debugging of each transformation by broadcasting it to tf @@ -113,14 +118,11 @@ namespace ur_calibrated_pose_pub complete_transformation_matrix = complete_transformation_matrix * dh_transformation.getTransformationMatrix(); } } + complete_transformation_matrix = complete_transformation_matrix * this->tcp_offset_transform_; - // Get namespace of the node: - std::string node_namespace = ros::this_node::getNamespace(); - - geometry_msgs::PoseStamped ur_calibrated_pose_msg; ur_calibrated_pose_msg.header.stamp = ros::Time::now(); - ur_calibrated_pose_msg.header.frame_id = node_namespace + "/base"; // TODO: change to generic name + ur_calibrated_pose_msg.header.frame_id = parent_frame_id; ur_calibrated_pose_msg.pose.position.x = complete_transformation_matrix(0, 3); ur_calibrated_pose_msg.pose.position.y = complete_transformation_matrix(1, 3); ur_calibrated_pose_msg.pose.position.z = complete_transformation_matrix(2, 3); @@ -150,7 +152,7 @@ namespace ur_calibrated_pose_pub q.setW(eigen_q.w()); transform.setRotation(q); - this->end_effector_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), node_namespace + "/base", node_namespace + "/calibrated_ee_pose")); // change to generic name + this->end_effector_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), parent_frame_id, child_frame_id)); ros::spinOnce(); publish_rate.sleep(); @@ -183,6 +185,80 @@ namespace ur_calibrated_pose_pub return; } + this->private_nh_.param("base_frame_id", this->base_frame_id_, ""); + this->base_frame_id_ = this->sanitizeFrameId(this->base_frame_id_); + + std::vector tcp_offset_vector(6, 0.0); + if(this->private_nh_.hasParam("tcp_offset")) + { + XmlRpc::XmlRpcValue tcp_offset_xml; + this->private_nh_.getParam("tcp_offset", tcp_offset_xml); + if(tcp_offset_xml.getType() == XmlRpc::XmlRpcValue::TypeArray && tcp_offset_xml.size() == 6) + { + for(int index = 0; index < tcp_offset_xml.size(); index++) + { + if(tcp_offset_xml[index].getType() == XmlRpc::XmlRpcValue::TypeDouble || + tcp_offset_xml[index].getType() == XmlRpc::XmlRpcValue::TypeInt) + { + double value = 0.0; + if(tcp_offset_xml[index].getType() == XmlRpc::XmlRpcValue::TypeDouble) + { + value = static_cast(tcp_offset_xml[index]); + } + else + { + value = static_cast(tcp_offset_xml[index]); + } + tcp_offset_vector[index] = value; + ROS_WARN_STREAM("tcp_offset[" << index << "] = " << value); + } + else + { + ROS_ERROR("tcp_offset parameter list is not well formed"); + return; + } + } + } + else if (tcp_offset_xml.getType() == XmlRpc::XmlRpcValue::TypeString) + { + // Some callers pass the list as a single string (e.g. "[0,0,0.6,0,0,0]"). Try to parse that. + std::string s = static_cast(tcp_offset_xml); + // remove brackets if present + if(!s.empty() && s.front() == '[' && s.back() == ']') + { + s = s.substr(1, s.size() - 2); + } + std::stringstream ss(s); + std::string item; + int idx = 0; + while (std::getline(ss, item, ',') && idx < 6) + { + try + { + tcp_offset_vector[idx] = std::stod(item); + } + catch(...) { + ROS_ERROR("Failed to parse tcp_offset string element to double"); + } + idx++; + } + if(idx != 6) + { + ROS_ERROR("tcp_offset string did not contain 6 values"); + } + } + else + { + ROS_ERROR("tcp_offset parameter must contain exactly 6 numeric values (array or string)"); + } + } + this->tcp_offset_transform_ = this->buildTransformFromOffset(tcp_offset_vector); + // Log the parsed tcp_offset for visibility + ROS_INFO_STREAM("tcp_offset parsed: [" << tcp_offset_vector[0] << ", " << tcp_offset_vector[1] << ", " << tcp_offset_vector[2] + << ", " << tcp_offset_vector[3] << ", " << tcp_offset_vector[4] << ", " << tcp_offset_vector[5] << "]"); + ROS_INFO_STREAM("tcp_offset transform translation: x=" << this->tcp_offset_transform_(0,3) + << ", y=" << this->tcp_offset_transform_(1,3) << ", z=" << this->tcp_offset_transform_(2,3)); + // Get DH parameters from parameter server XmlRpc::XmlRpcValue dh_param_list; this->private_nh_.getParam("dh_params", dh_param_list); @@ -345,4 +421,35 @@ namespace ur_calibrated_pose_pub } } } + + Eigen::Matrix4d URCalibratedPosePub::buildTransformFromOffset(const std::vector& offset_vector) const + { + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + if(offset_vector.size() != 6) + { + return transform; + } + + transform(0, 3) = offset_vector[0]; + transform(1, 3) = offset_vector[1]; + transform(2, 3) = offset_vector[2]; + + Eigen::AngleAxisd roll(offset_vector[3], Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch(offset_vector[4], Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd yaw(offset_vector[5], Eigen::Vector3d::UnitZ()); + Eigen::Matrix3d rotation = (roll * pitch * yaw).matrix(); + transform.block<3,3>(0, 0) = rotation; + + return transform; + } + + std::string URCalibratedPosePub::sanitizeFrameId(const std::string& frame_id) const + { + std::string sanitized = frame_id; + while(!sanitized.empty() && sanitized.front() == '/') + { + sanitized.erase(0, 1); + } + return sanitized; + } } diff --git a/ur/ur_launch_hardware/calibration/calibration_UR16e_09.yaml b/ur/ur_launch_hardware/calibration/calibration_UR10_09.yaml similarity index 100% rename from ur/ur_launch_hardware/calibration/calibration_UR16e_09.yaml rename to ur/ur_launch_hardware/calibration/calibration_UR10_09.yaml diff --git a/ur/ur_launch_hardware/calibration/calibration_UR10_12.yaml b/ur/ur_launch_hardware/calibration/calibration_UR10_12.yaml index bc44b3d1..90690fe9 100644 --- a/ur/ur_launch_hardware/calibration/calibration_UR10_12.yaml +++ b/ur/ur_launch_hardware/calibration/calibration_UR10_12.yaml @@ -2,43 +2,43 @@ kinematics: shoulder: x: 0 y: 0 - z: 0.1807705309236227 + z: 0.1809862716030697 roll: -0 pitch: 0 - yaw: 3.670441189734808e-07 + yaw: -2.119997983759947e-08 upper_arm: - x: 7.792706034762448e-05 + x: 0.0001189521963635788 y: 0 z: 0 - roll: 1.570182430060444 + roll: 1.570445716473262 pitch: 0 - yaw: -3.240294391066228e-06 + yaw: -3.600463888418717e-06 forearm: - x: -0.612348887349184 + x: -0.6123504837479578 y: 0 z: 0 - roll: 0.0003347388090885421 - pitch: 0.0001799609186808786 - yaw: 8.713106782103496e-07 + roll: 0.0006769015623701068 + pitch: 5.785713319325204e-05 + yaw: 3.811088287783351e-06 wrist_1: - x: -0.5713434949853855 - y: 0.001399796089719172 - z: 0.1739834748141271 - roll: 3.133547257962987 - pitch: 3.141484008718156 - yaw: 3.141590689544555 + x: -0.5713612706428757 + y: -0.001166854731704434 + z: 0.1737512475113588 + roll: 0.006715561511804876 + pitch: -0.00052588993217618 + yaw: -4.565657863408499e-06 wrist_2: - x: 9.49849898286023e-05 - y: -0.1197970691632824 - z: 0.0001197264669700888 - roll: 1.569796916475892 + x: -3.243228196583153e-05 + y: -0.1197274842566785 + z: 0.0001361725920662687 + roll: 1.569658972793055 pitch: 0 - yaw: -6.681882711231933e-07 + yaw: -5.514603827038583e-07 wrist_3: - x: 5.331657116612276e-05 - y: 0.1155987128858072 - z: -2.657717171409233e-06 - roll: 1.570773335905945 + x: 3.28141474464505e-05 + y: 0.1156129492667724 + z: 2.171798062353217e-05 + roll: 1.570984177554745 pitch: 3.141592653589793 - yaw: 3.141592643924699 - hash: calib_14062973764331575412 \ No newline at end of file + yaw: -3.141592518565552 + hash: calib_11593840841108551959 \ No newline at end of file diff --git a/ur/ur_launch_hardware/calibration/calibration_UR16e_13.yaml b/ur/ur_launch_hardware/calibration/calibration_UR10_13.yaml similarity index 100% rename from ur/ur_launch_hardware/calibration/calibration_UR16e_13.yaml rename to ur/ur_launch_hardware/calibration/calibration_UR10_13.yaml diff --git a/ur/ur_launch_hardware/launch/dual_ur.launch b/ur/ur_launch_hardware/launch/dual_ur.launch index be364ff8..18deaad2 100644 --- a/ur/ur_launch_hardware/launch/dual_ur.launch +++ b/ur/ur_launch_hardware/launch/dual_ur.launch @@ -13,6 +13,8 @@ + + @@ -24,6 +26,8 @@ + + diff --git a/ur/ur_launch_hardware/scripts/ur_twist_limiter.py b/ur/ur_launch_hardware/scripts/ur_twist_limiter.py index 7763aecb..135ca7aa 100755 --- a/ur/ur_launch_hardware/scripts/ur_twist_limiter.py +++ b/ur/ur_launch_hardware/scripts/ur_twist_limiter.py @@ -187,13 +187,13 @@ def setup_ddynamic_reconfigure(self): ddynrec = DDynamicReconfigure("example_dyn_rec") # Add variables (name, description, default value, min, max, edit_method) - ddynrec.add_variable("lin_vel_limit", "float/double variable", 0.15, 0, 0.3) - ddynrec.add_variable("angular_vel_limit", "float/double variable", 0.2, 0, 0.6) + ddynrec.add_variable("lin_vel_limit", "float/double variable", 0.2, 0, 0.4) + ddynrec.add_variable("angular_vel_limit", "float/double variable", 0.3, 0, 0.6) ddynrec.add_variable("lin_acc_limit", "float/double variable", 4.0, 0, 10.0) ddynrec.add_variable("angular_acc_limit", "float/double variable", 7.0, 0, 14.0) ddynrec.add_variable("lin_jerk_limit", "float/double variable", 0.8, 0, 2.0) ddynrec.add_variable("angular_jerk_limit", "float/double variable", 1.7, 0, 3.0) - ddynrec.add_variable("command_timeout", "float/double variable", 0.05, 0, 0.5) + ddynrec.add_variable("command_timeout", "float/double variable", 0.1, 0, 0.5) # Start the server ddynrec.start(self.dyn_rec_callback) diff --git a/ur/ur_msgs b/ur/ur_msgs index 7f730650..6c6dc66c 160000 --- a/ur/ur_msgs +++ b/ur/ur_msgs @@ -1 +1 @@ -Subproject commit 7f73065036ea8acbf3a0551f480cc57493341f22 +Subproject commit 6c6dc66c63bd551e3985281739add762bfaa3aa9 diff --git a/ur/ur_ros_driver b/ur/ur_ros_driver index 2db30695..b3517c16 160000 --- a/ur/ur_ros_driver +++ b/ur/ur_ros_driver @@ -1 +1 @@ -Subproject commit 2db30695d3ee311fb4bf4e195b04e19185851cde +Subproject commit b3517c16f83943eef8e9a9377bbae98b312f4b0f