commit c3459040315a70c49af8b150b300f50536cd425c Author: Niko Date: Wed Aug 30 15:46:39 2023 +0200 initial commit diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/Fanuc_ROS1_ROS2.iml b/.idea/Fanuc_ROS1_ROS2.iml new file mode 100644 index 0000000..74d515a --- /dev/null +++ b/.idea/Fanuc_ROS1_ROS2.iml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..f9fa219 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..255daa3 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..95b08d8 --- /dev/null +++ b/README.md @@ -0,0 +1,443 @@ +# CPS Hub: Fanuc CRX-10iA Framework + +## Installation of the core packages + +The following software needed to be installed on the Intel NUC: + ++ Ubuntu 20.04 ++ ROS-1 Noetic (from binaries) ++ ROS-2 Foxy (from source) including a separate initialization of the ROS-1 Bridge ++ Fanuc ROS-1 Interface (Fanuc Driver) + +### ROS-1 Noetic +The installation of ROS-1 Noetic is the most convenient option as it can be done using binaries. The following steps need to be executed in a bash terminal: + +``` +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + +sudo apt install curl + +curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + +sudo apt update + +sudo apt install ros-noetic-desktop-full +``` + +To use ROS-1 in a bash terminal, the setup file must be sourced. This must be done in each new bash terminal window. + +``` +source /opt/ros/noetic/setup.bash +``` + +### ROS-2 Foxy + +Installing ROS-2 Foxy requires more steps because it needs to be installed from source. This installation method is necessary to set up the ROS-1 Bridge for custom message types. The following steps must be followed in a bash terminal: + +``` +sudo apt update && sudo apt install locales +sudo locale-gen en_US en_US.UTF-8 +sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +export LANG=en_US.UTF-8 + +sudo apt install software-properties-common +sudo add-apt-repository universe + +sudo apt update && sudo apt install curl -y +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + +echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + +sudo apt update && sudo apt install -y libbullet-dev python3-pip python3-pytest-cov ros-dev-tools +python3 -m pip install -U argcomplete flake8-blind-except flake8-builtins flake8-class-newline flake8-comprehensions flake8-deprecated flake8-docstrings flake8-import-order flake8-quotes pytest-repeat pytest-rerunfailures pytest +sudo apt install --no-install-recommends -y libasio-dev libtinyxml2-dev +sudo apt install --no-install-recommends -y libcunit1-dev + +mkdir -p ~/ros2_foxy/src +cd ~/ros2_foxy +vcs import --input https://raw.githubusercontent.com/ros2/ros2/foxy/ros2.repos src + +sudo apt upgrade + +sudo rosdep init +rosdep update +rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-5.3.1 urdfdom_headers" + +cd ~/ros2_foxy/ +colcon build --symlink-install +``` + +In any bash terminal, the ROS-2 setup file must be sourced if you intend to use ROS-2. + +``` +$ source ~/ros2_foxy/install/local_setup.bash +``` + +### Overlay Workspaces +To install the additional packages, overlay workspaces are needed. + +``` +source /path/to/noetic/setup.bash +cd ~ + +mkdir +cd +mkdir src + +catkin_make +``` + +In another bash terminal: + +``` +source /path/to/foxy/setup.bash +cd ~ + +mkdir +cd +mkdir src + +colcon build +``` + +Now you can place the ROS-1 and ROS-2 core packages from this repository into the "src" directories of the overlay workspaces: + ++ ros1_noetic_core (into the ROS-1 overlay workspace folder on the NUC): + + fanuc_msg_translator + + fanuc_msg_translator_msgs ++ ros2_foxy_core (into the ROS-2 overlay workspace folder on the NUC): + + fanuc_crx_description + + fanuc_msg_translator_msgs + +CAVE: all python files have to be made executable: + + +``` +cd path/to/python/file/in/the/package +chmod +x +``` + + +### Fanuc ROS-1 Interface +The Fanuc ROS-1 Interface package can be acquired by requesting it from Fanuc's Technical Support. Fanuc's Technical Support will provide a folder containing the pre-built ROS-1 package along with a short documentation. + +The zip package containing the Fanuc ROS-1 Interface must be extracted and placed in the file system. It is common to place it in the user's folder. + +Before installing the Fanuc ROS-1 Interface, it is advisable to create a ROS-1 workspace if one does not already exist. The following bash code demonstrates how to create this workspace in the user's home directory: + + +``` +source /opt/ros/noetic/setup.bash + +cd ~ +mkdir + +cd +mkdir src + +catkin_make +``` + +For the installation process, the following steps need to be executed in a bash terminal: + +``` +sudo apt install ros-noetic-industrial-msgs +sudo apt install ros-noetic-rviz + +sudo apt install libmodbus-dev libjsoncpp-dev lshw + +chmod u+x path/to/fanuc/ros/driver/lib/fanuc_ros_driver/*_node +chmod u+x path/to/fanuc/ros/driver/_setup_util.py + +source path/to/fanuc/ros/driver/setup.bash + +cd path/to/your/ROS1/workspace +rm -rf devel build +catkin_make +source devel/setup.bash +``` + +Running these commands will install the Fanuc ROS-1 interface. However, the license is still missing. Therefore, a file containing the hardware information of the computer must be sent to Fanuc's technical support. + +To create a hardware information file, the following commands need to be executed in a bash terminal: +``` +roslaunch fanuc_ros_driver fanuc_interface.launch create_license:=true +``` + +Once the license file from technical support is received, the Fanuc ROS-1 Interface can be launched using the following steps: + +``` +roslaunch fanuc_ros_driver fanuc_interface.launch ip:="robot ip address" robot_type:="CRX-10iA" license:="path to license.data" +``` + +### ROS-1 Bridge + +CAVE: Before installing the ROS-1 Bridge, the core packages from this repository must be placed into the correct workspaces (and the workspaces have to be built with catkin for ROS-1 and colcon for ROS-2). + + +The installation files for the ROS-1 Bridge are already included in the ROS-2 distribution repository. To initialize the ROS-1 Bridge, the following steps need to be executed in a bash terminal: + + +``` +cd ~/ros2_foxy + +colcon build --symlink-install --packages-skip ros1_bridge + +source path/to/ros1/workspace/install/setup.bash +source path/to/ros2/workspace/install/setup.bash + +source path/to/ros1/overlay/workspace/install/setup.bash +source path/to/ros2/overlay/workspace/install/local_setup.bash + +colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure +``` + +### License Path + +At this step, the core packages from this repository should already be installed. Now, the static path of the license should be written into the FANUC_LICENSE environment variable. This can be permanently done by adding the export statement to the .bashrc file. + +``` +echo 'export FANUC_LICENSE="/absolute/path/to/license.data" ' >> ~/.bashrc +``` + +## Launching the Interface +### ROS-1 fanuc_msg_translator Package +Depending on the use case following launch files are available: + ++ fanuc_1.launch ++ fanuc_2.launch ++ fanuc_pair.launch (launches the nodes for the operation of both robots on the same NUC) + +Example: +``` +source /path/to/ros_1_overlay_ws/devel/setup.bash + +roslaunch fanuc_msg_translator fanuc_1.launch +``` + +These launch files launch all the nodes seen in this figure, including the parameter for the ROS-1 Bridge: + +![Alt text](/imgs/landscape_full_interface_ros1.png) + +### ROS-2 ros1-bridge Package +To enable communication between ROS-1 and ROS-2 + +``` +source /path/to/ros_2_overlay_ws/install/setup.bash + +ros2 run ros1_bridge parameter_bridge +``` + +### ROS-2 fanuc_crx_descripton Package + +In ROS 1, the visualization cannot be directly bridged to ROS 2. As a result, the robot_state_publisher and some other nodes have to be launched separately in ROS 2. Additionally, the /tf and /tf_static topics are not bridged, allowing ROS 2 nodes to create them without encountering any complications. The following launch files can be launched: + ++ fanuc_1.launch.py ++ fanuc_2.launch.py + +``` +source /path/to/ros_2_overlay_ws/install/setup.bash + +ros2 launch fanuc_crx_description fanuc_1.launch.py +``` + +The following figure shows the node structure in ROS-2: + +![Alt text](/imgs/landscape_full_interface_ros2.png) + +## Functionality of the Interface +Once the interface is launched, it offers the following functions. + +### Joint States +The current joint state values of the robot can be received from the /joint_states topic. + +### Robot Status +The current robot status can be be received from the /robot_status topic. The custom message type has the following message file (fanuc_msg_translator_msgs package: FanucRobotStatus.msg): +``` +std_msgs/Header header + +fanuc_msg_translator_msgs/FanucRobotStatusTriState tp_enabled + +fanuc_msg_translator_msgs/FanucRobotStatusTriState e_stopped + +fanuc_msg_translator_msgs/FanucRobotStatusTriState drives_powered + +fanuc_msg_translator_msgs/FanucRobotStatusTriState motion_possible + +fanuc_msg_translator_msgs/FanucRobotStatusTriState in_motion + +fanuc_msg_translator_msgs/FanucRobotStatusTriState in_error + +int32 error_code +``` + +It uses the following message type, which is also defined in the same package (fanuc_msg_translator_msgs package: FanucRobotStatusTriState.msg): + +``` +int8 val + +# enumerated values + +# Unknown or unavailable +int8 UNKNOWN=-1 + +# High state +int8 TRUE=1 +int8 ON=1 +int8 ENABLED=1 +int8 HIGH=1 +int8 CLOSED=1 + +# Low state +int8 FALSE=0 +int8 OFF=0 +int8 DISABLED=0 +int8 LOW=0 +int8 OPEN=0 +``` + +### Set Payload +To set a predefined payload, a message of the following message type has to be sent to the /bridge/set_payload_num/goal topic (fanuc_msg_translator_msgs package: FanucPayloadStatus.msg). + +``` +std_msgs/Header header + +int16 payload_num +``` + +### Get the current Payload Number +The last set payload number is periodically published to the /bridge/set_payload_num/state topic (fanuc_msg_translator_msgs package: FanucPayloadStatus.msg). + +``` +std_msgs/Header header + +int16 payload_num +``` + +### Send Trajectory +A goal trajectory has to be sent to the /bridge/follow_joint_trajectory/goal topic. It uses the JointTrajectory message type from the trajectory_msgs package. + +### Trajectory Queue +The trajectories, which are waiting for execution, are shown in the /bridge/follow_joint_trajectory/queue topic, which periodically publishes messages of the following message type (fanuc_msg_translator_msgs package: FanucTrajectoryConverterQueue.msg): + +``` +std_msgs/Header header + +fanuc_msg_translator_msgs/FanucTrajectoryConverterQueueItem[] goals +``` + +It uses the following message type, which is also defined in the same package (fanuc_msg_translator_msgs package: FanucTrajectoryConverterQueueItem.msg): + + +``` +builtin_interfaces/Time received_time_stamp + +int16 status_in_queue + +int16 status_on_server +``` + +The received_time_stamp is the unique id of every trajectory in the queue. It is the time stamp from the header of the trajectory message received via the /bridge/follow_joint_trajectory/goal topic. + +The status in the queue is: ++ 0 if the trajectory is waiting ++ 1 if the trajectory is currently executed ++ 100 to 130 if the trajectory is finished (counter until 130, afterward it is removed from the queue topic) + +The status on the server is: ++ -1 if the trajectory is waiting or currently executed ++ one of the following values from the original message type (action server of the Fanuc ROS Driver) after the trajectory is finished or aborted. + +From: http://docs.ros.org/en/api/actionlib_msgs/html/msg/GoalStatus.html +``` + GoalID goal_id +uint8 status +uint8 PENDING = 0 # The goal has yet to be processed by the action server +uint8 ACTIVE = 1 # The goal is currently being processed by the action server +uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing + # and has since completed its execution (Terminal State) +uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State) +uint8 ABORTED = 4 # The goal was aborted during execution by the action server due + # to some failure (Terminal State) +uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, + # because the goal was unattainable or invalid (Terminal State) +uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing + # and has not yet completed execution +uint8 RECALLING = 7 # The goal received a cancel request before it started executing, + # but the action server has not yet confirmed that the goal is canceled +uint8 RECALLED = 8 # The goal received a cancel request before it started executing + # and was successfully cancelled (Terminal State) +uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be + # sent over the wire by an action server +``` + + + + +### Cancel Trajectory +A trajectory can be canceled by sending of the following type to the /bridge/follow_joint_trajectory/cancel topic (fanuc_msg_translator_msgs package: FanucTrajectoryConverterCancel.msg). + +``` +std_msgs/Header header + +builtin_interfaces/Time received_time_stamp +``` + +## Operation Modes +### IP Configuration +The following static IP addresses are assigned: + ++ Fanuc 1: 192.168.1.201 ++ Fanuc 2: 192.168.1.206 ++ NUC 1: 192.168.1.202 ++ NUC 2: 192.168.1.207 + +If the IP addresses of the robot controllers are changed, the IP addresses have to be changed in following package: + ++ Workspace: ROS-1 Overlay ++ Package: fanuc_msg_translator ++ Config Files: params_fanuc_1.yaml and params_fanuc_2.yaml + +### Modularity +Because of the static IPs and the Fanuc Robots and the NUCs can be used flexibly: + ++ 1 Robot - 1 NUC ++ 2 Robots - 1 NUC + +![Alt text](/imgs/operation_mode_one_nuc.png) ++ 2 Robots - 2 NUCs (in the same network, e.g. CPS-HUB) + +![Alt text](/imgs/operation_mode_two_nuc.png) ++ 2 Robots - 2 NUCs (in different networks --> mobile router must be configured identically to the CPS Laboratory networks router) + +![Alt text](/imgs/operation_mode_split.png) + + + + + +## ROS-2 Network control + +To use to Fanuc ROS-2 Interface from another computer than the NUC, the message types need to be installed to ROS-2. Therefore copy the fanuc_msg_translator_msgs package from the ros2_foxy_core folder of this repository into the overlay workspace on any other machine and install it with colcon build. + +## RViz +The /tf and the /robot_description topics are published by the nodes launched in the procedure shown above. If you want to use RViz on another computer in the same network, make sure the fanuc_crx_description is installed in the local overlay workspace. If ROS-2 Humble is used on the computer,the fanuc_crx_description package from the folder "ros2_humble_crx_description" +can be utilized. If ROS-2 Foxy is used, please use the package with the same name from the "ros2_foxy_core" folder, because ROS-2 Foxy has issues compiling the xacro (urdf) files. + +You can start rviz with: +``` +source /path/to/ROS-2/setup.bash + +rviz2 +``` + +To visualize the transformation of the frames click "add" --> "TF". To show the whole model of the robot click "add" --> "RobotModel". + +TF and RobotModel should be shown in the tree structure on the left side of RViz. Make sure following settings are made for the RobotModel: ++ Description Topic: //robot_description ++ TF Prefix: + +![Alt text](/imgs/rviz.png) + + + diff --git a/fanuc_licenses/license_nuc1.data b/fanuc_licenses/license_nuc1.data new file mode 100644 index 0000000..4c6526d --- /dev/null +++ b/fanuc_licenses/license_nuc1.data @@ -0,0 +1 @@ +Dejk049IFca6vvB5Or3aDFH5/kVhdjBqTA0kuZKs0JW0ZwWoOypgXECZxtYc3w7Sc71eRa1iiMyxOPtDDyNusxLCjVdDmecgyi+K0+6emuuI3WkwU56wzxSLtlq45/8Wnz6Omix3066/eXCLv8pVnjPrXmvYYYt3CSKWSimobCPwVBga0+a0xjcIc9O47JAuMoxbUi7g20fJl1UXtUQ6eIeWEfpZ81prEtsdyaSvZKMMxeuOUdn3SiYtLe04/9E6fQUZ+qCBpKaLuobjf/SajQcgpaq+X+9U7m4eQa0zQnBZzpxHj3qbL9HJMc4Bp0wuq3lL+PhZd3jZBXmAHSl0Zg== \ No newline at end of file diff --git a/fanuc_licenses/license_nuc2.data b/fanuc_licenses/license_nuc2.data new file mode 100644 index 0000000..292976a --- /dev/null +++ b/fanuc_licenses/license_nuc2.data @@ -0,0 +1 @@ +Nd2ivgRlJZETuI/xfa/Li9tkNQ4mrfDjciAK6Tz932YrnTWo2lAbLX/G+8Xl5aAGVnarySlX3deJd1ZYegGuFEmgIT2hXDSIFSSr9aWOs3KP++cpsZIDG1AVsdgvT6DCPJ2gth0u49H5Fl2ICfxqL0WoRGKh1HxWAxFyiD6kNlhQFLK4Zp8XkP8tH9a0fiImav/kgm3PEUCF1gwg2TBPmkWcNv8RpAvg/R67fgRh+uouq1rkU8vdMHbADHoCIWZ8j032ir5jvzvmJoBHiebRhWrSvVd9jqYRTztwvK7EpsSdcLb9ZPqgRwNxVzN3EJcooH0gDU82HuNE1j7GPMRH1w== \ No newline at end of file diff --git a/imgs/landscape_full_interface_ros1.png b/imgs/landscape_full_interface_ros1.png new file mode 100644 index 0000000..e2608f3 Binary files /dev/null and b/imgs/landscape_full_interface_ros1.png differ diff --git a/imgs/landscape_full_interface_ros2.png b/imgs/landscape_full_interface_ros2.png new file mode 100644 index 0000000..02c09ac Binary files /dev/null and b/imgs/landscape_full_interface_ros2.png differ diff --git a/imgs/operation_mode_one_nuc.png b/imgs/operation_mode_one_nuc.png new file mode 100644 index 0000000..6a23ec3 Binary files /dev/null and b/imgs/operation_mode_one_nuc.png differ diff --git a/imgs/operation_mode_split.png b/imgs/operation_mode_split.png new file mode 100644 index 0000000..32b1719 Binary files /dev/null and b/imgs/operation_mode_split.png differ diff --git a/imgs/operation_mode_two_nuc.png b/imgs/operation_mode_two_nuc.png new file mode 100644 index 0000000..1c2bf9b Binary files /dev/null and b/imgs/operation_mode_two_nuc.png differ diff --git a/imgs/rviz.png b/imgs/rviz.png new file mode 100644 index 0000000..5ff4a9d Binary files /dev/null and b/imgs/rviz.png differ diff --git a/ros1_noetic_core/fanuc_msg_translator/CMakeLists.txt b/ros1_noetic_core/fanuc_msg_translator/CMakeLists.txt new file mode 100644 index 0000000..1e00414 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(fanuc_msg_translator) + +## 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 COMPONENTS + rospy + fanuc_msgs +) + +## 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 fanuc_msg_translator +# CATKIN_DEPENDS rospy +# 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}/fanuc_msg_translator.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/fanuc_msg_translator_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_fanuc_msg_translator.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/ros1_noetic_core/fanuc_msg_translator/config/params_bridge.yaml b/ros1_noetic_core/fanuc_msg_translator/config/params_bridge.yaml new file mode 100644 index 0000000..37f2aad --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/config/params_bridge.yaml @@ -0,0 +1,59 @@ +topics: + - + topic: /fanuc_1/joint_states + type: sensor_msgs/msg/JointState + queue_size: 10 + - + topic: /fanuc_1/bridge/follow_joint_trajectory/cancel + type: fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel + queue_size: 10 + - + topic: /fanuc_1/bridge/follow_joint_trajectory/goal + type: trajectory_msgs/msg/JointTrajectory + queue_size: 10 + - + topic: /fanuc_1/bridge/follow_joint_trajectory/queue + type: fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue + queue_size: 10 + - + topic: /fanuc_1/bridge/set_payload_num/goal + type: fanuc_msg_translator_msgs/msg/FanucPayloadNum + queue_size: 10 + - + topic: /fanuc_1/bridge/set_payload_num/state + type: fanuc_msg_translator_msgs/msg/FanucPayloadNum + queue_size: 10 + - + topic: /fanuc_1/bridge/robot_status + type: fanuc_msg_translator_msgs/msg/FanucRobotStatus + queue_size: 10 + - + topic: /fanuc_2/joint_states + type: sensor_msgs/msg/JointState + queue_size: 10 + - + topic: /fanuc_2/bridge/follow_joint_trajectory/cancel + type: fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel + queue_size: 10 + - + topic: /fanuc_2/bridge/follow_joint_trajectory/goal + type: trajectory_msgs/msg/JointTrajectory + queue_size: 10 + - + topic: /fanuc_2/bridge/follow_joint_trajectory/queue + type: fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue + queue_size: 10 + - + topic: /fanuc_2/bridge/set_payload_num/goal + type: fanuc_msg_translator_msgs/msg/FanucPayloadNum + queue_size: 10 + - + topic: /fanuc_2/bridge/set_payload_num/state + type: fanuc_msg_translator_msgs/msg/FanucPayloadNum + queue_size: 10 + - + topic: /fanuc_2/bridge/robot_status + type: fanuc_msg_translator_msgs/msg/FanucRobotStatus + queue_size: 10 + + diff --git a/ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_1.yaml b/ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_1.yaml new file mode 100644 index 0000000..d5b7f9b --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_1.yaml @@ -0,0 +1,3 @@ +robot_ip: "192.168.1.201" +robot_port: "502" + diff --git a/ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_2.yaml b/ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_2.yaml new file mode 100644 index 0000000..870f3c5 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_2.yaml @@ -0,0 +1,3 @@ +robot_ip: "192.168.1.206" +robot_port: "502" + diff --git a/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_1.launch b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_1.launch new file mode 100644 index 0000000..e7ba918 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_1.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_2.launch b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_2.launch new file mode 100644 index 0000000..3762f49 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_2.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_1.launch b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_1.launch new file mode 100644 index 0000000..41bc3f2 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_1.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_2.launch b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_2.launch new file mode 100644 index 0000000..15c81f2 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_2.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_pair.launch b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_pair.launch new file mode 100644 index 0000000..7ca9fa2 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/launch/fanuc_pair.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/ros1_noetic_core/fanuc_msg_translator/package.xml b/ros1_noetic_core/fanuc_msg_translator/package.xml new file mode 100644 index 0000000..aed2fa7 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/package.xml @@ -0,0 +1,68 @@ + + + fanuc_msg_translator + 0.0.0 + The fanuc_msg_translator package + + + + + fanuc1 + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + rospy + rospy + fanuc_msgs + industrial_msgs + actionlib + trajectory_msgs + control_msgs + sensor_msgs + std_msgs + + + + + + + diff --git a/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_joint_states_converter_node.py b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_joint_states_converter_node.py new file mode 100644 index 0000000..21105a5 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_joint_states_converter_node.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 + +import rospy +from sensor_msgs.msg import JointState + +# Namespace of the node +ns = rospy.get_namespace() + +joint_states_publisher = rospy.Publisher(ns + 'bridge/joint_states', JointState, queue_size=10) + + +def joint_states_callback(msg): + """ + Callback function for the /joint_states topic. Publishes the received message to the /bridge/joint_states topic. + + :param msg: The received message from the /joint_states topic + :type msg: JointState + """ + global joint_states_publisher + + joint_states_publisher.publish(msg) + + +def main(): + # Initialize the node + rospy.init_node('fanuc_joint_states_converter_node') + + # Subscribe to the /robot_status topic + rospy.Subscriber(ns + 'joint_states', JointState, joint_states_callback) + + rospy.loginfo("[Joint States Converter] Node initialized!") + + # Spin the node + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_payload_converter_node.py b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_payload_converter_node.py new file mode 100644 index 0000000..07ee831 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_payload_converter_node.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 + +import rospy + +from fanuc_msgs.srv import SetPayloadNum +from industrial_msgs.msg import RobotStatus +from fanuc_msg_translator_msgs.msg import FanucPayloadNum + +# Namespace of the node +ns = rospy.get_namespace() + +robot_status = RobotStatus() +payload_status = -1 + +payload_status_publisher = rospy.Publisher(ns + 'bridge/set_payload_num/state', FanucPayloadNum, queue_size=10) + + +def set_payload_callback(msg): + """ + Callback function for the /set_payload topic. Calls the /set_payload_num service with the received value if the + robot is in the correct state. + + :param msg: The received message from the /set_payload topic + :type msg: FanucPayloadNum + """ + global payload_status + + # Create a service client for /set_payload_num + rospy.wait_for_service(ns + 'set_payload_num') + try: + # Create a proxy to call the service + set_payload_num = rospy.ServiceProxy(ns + 'set_payload_num', SetPayloadNum) + + if robot_status.mode.val == 2 \ + and robot_status.e_stopped.val == 0 \ + and robot_status.drives_powered.val == 1 \ + and robot_status.motion_possible.val == 1 \ + and robot_status.in_motion.val == 0 \ + and robot_status.in_error.val == 0 \ + and robot_status.error_code == 0: + # Call the service with the received value from /set_payload topic + set_payload_num(msg.payload_num) + set_payload_num.wait_for_service() + payload_status = msg.payload_num + rospy.loginfo("[Payload Converter] SetPayloadNum service called with payload: " + str(payload_status)) + + else: + rospy.loginfo("[Payload Converter] Robot is not in the correct state to set payload") + + except rospy.ServiceException as e: + # Log the error if the service call failed + rospy.logerr("[Payload Converter] Service call failed: %s", str(e)) + + +def payload_status_publisher_callback(event): + """ + Callback function for the timer that publishes the payload status every 0.2 seconds. + """ + global payload_status + + # Create a new FanucRobotStatus message + payload_status_msg = FanucPayloadNum() + + # Set the values of the new message + payload_status_msg.header.stamp = rospy.Time.now() + payload_status_msg.payload_num = payload_status + + # Publish the new message + payload_status_publisher.publish(payload_status_msg) + + +def robot_status_callback(msg): + """ + Callback function for the /robot_status topic. Updates the robot_status variable with the received message. + """ + global robot_status + + # Update the robot_status variable + robot_status = msg + + +def main(): + # Initialize the node + rospy.init_node('fanuc_payload_converter_node') + + # Subscribe to the /set_payload topic + rospy.Subscriber(ns + 'bridge/set_payload_num/goal', FanucPayloadNum, set_payload_callback) + rospy.Subscriber(ns + 'robot_status', RobotStatus, robot_status_callback) + + # Create a timer to publish the payload status every 0.2 seconds + interval = rospy.Duration(0.2) + rospy.Timer(interval, payload_status_publisher_callback) + + rospy.loginfo("[Payload Converter] Node initialized!") + + # Spin the node + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_read_single_io_converter_node.py b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_read_single_io_converter_node.py new file mode 100644 index 0000000..c0f58c7 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_read_single_io_converter_node.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 + +import rospy + +from fanuc_msgs.srv import ReadSingleIO +from industrial_msgs.msg import RobotStatus +from std_msgs.msg import Int16 +from fanuc_msg_translator_msgs.msg import FanucIOState, FanucReadSingleIO + +io_publisher = rospy.Publisher('/msg_translator/io_status', FanucIOState, queue_size=10) + + +def read_single_io_callback(msg): + # Create a service client for /set_payload_num + rospy.wait_for_service('/read_single_io') + try: + # Create a proxy to call the service + read_single_io = rospy.ServiceProxy('/read_single_io', ReadSingleIO) + + value = read_single_io(0, 101) + + rospy.loginfo("[Read Single IO Converter] Value:" + str(value)) + + fanuc_io_state = FanucIOState() + fanuc_io_state.header.stamp = rospy.Time.now() + fanuc_io_state.type = msg.type + fanuc_io_state.address = msg.address + fanuc_io_state.value = value + + io_publisher.publish(value) + + rospy.loginfo("[Read Single IO Converter] IO Value: " + str(value)) + + except rospy.ServiceException as e: + # Log the error if the service call failed + rospy.logerr("[Read Single IO Converter] Service call failed: %s", str(e)) + + +def main(): + # Initialize the node + rospy.init_node('fanuc_read_single_io_converter_node') + + # Subscribe to the /set_payload topic + rospy.Subscriber('/msg_translator/read_single_io', FanucReadSingleIO, read_single_io_callback) + + # Spin the node + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_robot_status_converter_node.py b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_robot_status_converter_node.py new file mode 100644 index 0000000..e3405b0 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_robot_status_converter_node.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import rospy +from industrial_msgs.msg import RobotStatus, RobotMode +from fanuc_msg_translator_msgs.msg import FanucRobotStatus + +# Namespace of the node +ns = rospy.get_namespace() + +def robot_status_callback(msg): + """ + Callback function for the /robot_status topic. This function is called every time a new message is published on + the topic. The function converts the message from the industrial_msgs/RobotStatus message type to the custom + fanuc_msg_translator_msgs/FanucRobotStatus message type and publishes the converted message on the + /msg_translator/robot_status topic. + + :param msg: The message published on the /robot_status topic + :type msg: industrial_msgs/RobotStatus + """ + # Create a publisher for the result + result_publisher = rospy.Publisher(ns + 'bridge/robot_status', FanucRobotStatus, queue_size=10) + + # Create a new FanucRobotStatus message + robot_status = FanucRobotStatus() + + # Set the values of the new message + robot_status.header.stamp = msg.header.stamp + if msg.mode.val == RobotMode.MANUAL: + robot_status.tp_enabled.val = 1 + elif msg.mode.val == RobotMode.AUTO: + robot_status.tp_enabled.val = 0 + elif msg.mode.val == RobotMode.UNKNOWN: + robot_status.tp_enabled.val = -1 + robot_status.e_stopped = msg.e_stopped + robot_status.drives_powered = msg.drives_powered + robot_status.motion_possible = msg.motion_possible + robot_status.in_motion = msg.in_motion + robot_status.in_error = msg.in_error + robot_status.error_code = msg.error_code + + # Publish the new message + result_publisher.publish(robot_status) +def main(): + # Initialize the node + rospy.init_node('fanuc_robot_status_converter_node') + + # Subscribe to the /robot_status topic + rospy.Subscriber(ns + 'robot_status', RobotStatus, robot_status_callback) + + rospy.loginfo("[Robot Status Converter] Node initialized!") + + # Spin the node + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_trajectory_converter_node.py b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_trajectory_converter_node.py new file mode 100644 index 0000000..7df8778 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_trajectory_converter_node.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python3 + +import rospy +import actionlib +from trajectory_msgs.msg import JointTrajectory +from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal +from fanuc_msg_translator_msgs.msg import FanucTrajectoryConverterQueue, FanucTrajectoryConverterQueueItem, \ + FanucTrajectoryConverterCancel + +# Namespace of the node +ns = rospy.get_namespace() + +# Queue for all trajectories +queue = [] + +# Queue for all finished trajectories +cemetery = [] + +# Conntection to the action server of the fanuc_interface_node +client = actionlib.SimpleActionClient(ns + 'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) + +# Publisher for the queue (for ROS-2) +queue_publisher = rospy.Publisher(ns + 'bridge/follow_joint_trajectory/queue', FanucTrajectoryConverterQueue, + queue_size=10) + +# Marker for 500 ms delay in the queue handling, activated when a trajectory is canceled +canceled_trajectory = False + + +def goal_callback(trajectory): + """ + This function is called when a new trajectory is received. It adds the trajectory to the queue. + + :param trajectory: The received trajectory message + """ + global queue + rospy.loginfo("[Trajectory Converter] Trajectory received!") + trajectory_wrapper = TrajectoryWrapper(trajectory) + queue.append(trajectory_wrapper) + + +def cancel_callback(cancel): + """ + This function is called when a cancel command is received. It deletes the trajectory from the queue if it is not + sent to the action server yet. If it is sent to the action server, the goal is canceled and the trajectory is + moved to the cemetery. + + :param cancel: The received cancel message + """ + global client + global queue + global cemetery + global canceled_trajectory + + rospy.loginfo("[Trajectory Converter] Cancel command received!") + + for trajectory_wrapper in queue: + if trajectory_wrapper.get_trajectory().header.stamp == cancel.received_time_stamp: + if trajectory_wrapper.get_status() == 0: + trajectory_wrapper.set_status(100) + cemetery.append(queue.pop(queue.index(trajectory_wrapper))) + elif trajectory_wrapper.get_status() == 1: + client.cancel_all_goals() + canceled_trajectory = True + break + + +def done_callback(state, result): + """ + This function is called when the action server is done with the trajectory. It sets the status of the trajectory + in the queue to 100, moves it to the cemetery and sets the action status of the trajectory to the state of + the action server. + + :param state: The state of the action server + :param result: The result of the action server + """ + global queue + + rospy.loginfo("[Trajectory Converter] Action server done! State: " + str(state)) + + queue[0].set_action_status(state) + + +class TrajectoryWrapper: + """ + This class is used to wrap the trajectory message and add two variables to it: status and action_status. + + status: The status of the trajectory in the queue. + action_status: The status of the action server of the fanuc_interface_node. + """ + + def __init__(self, trajectory=None): + """ + The constructor of the TrajectoryWrapper class. + + :param trajectory: The trajectory message to be wrapped + """ + self.trajectory = trajectory + self.status = 0 + self.action_status = -1 + + def set_action_status(self, status): + """ + This function sets the action status of the trajectory. + + :param status: The action status to be set + """ + self.action_status = status + + def get_action_status(self): + """ + This function returns the action status of the trajectory. + + :return: The action status of the trajectory + """ + return self.action_status + + def set_status(self, status): + """ + This function sets the status of the trajectory. + + :param status: The status to be set + """ + self.status = status + + def get_status(self): + """ + This function returns the status of the trajectory. + + :return: The status of the trajectory + """ + return self.status + + def get_trajectory(self): + """ + This function returns the trajectory message. + + :return: The trajectory message + """ + return self.trajectory + + +def main(): + """ + This is the main function of the node. It initializes the node, subscribes to the topics and runs the queue handler. + + The queue hanlder is called every 0.1 seconds (while loop). It works like a state machine for the queue. + Trajectories can have the following states: + 0: Not started + 1: In progress + 2: Finished + 100: Finished and in the cemetery + 101 - 129: Finished and in the cemetery (timer) + 130: Finished and removed from the cemetery (not published to /msg_translator/follow_joint_trajectory/queue anymore) + + The action status of the trajectory is saved in the variable action_status. The action status is the status of the + action server of the fanuc_interface_node. The action status is -1 until the action server reaches a final state for + this goal. Then it changes to the resulting state of the action server. The action status can be found here: + http://docs.ros.org/en/api/actionlib_msgs/html/msg/GoalStatus.html + + This function also publishes the queue and the cemetery to the topic /msg_translator/follow_joint_trajectory/queue. + """ + + global client + global queue_publisher + global queue + global cemetery + global canceled_trajectory + + rospy.init_node('fanuc_trajectory_converter_node') + + rospy.Subscriber(ns + 'bridge/follow_joint_trajectory/goal', JointTrajectory, goal_callback, queue_size=10) + rospy.Subscriber(ns + 'bridge/follow_joint_trajectory/cancel', FanucTrajectoryConverterCancel, cancel_callback, + queue_size=10) + + client.wait_for_server() + + rospy.loginfo("[Trajectory Converter] Node initialized!") + + while not rospy.is_shutdown(): + if len(queue) > 0: + if queue[0].get_status() == 0 \ + and client.get_state() != actionlib.GoalStatus.ACTIVE \ + and client.get_state() != actionlib.GoalStatus.PENDING: + goal = FollowJointTrajectoryGoal() + goal.trajectory = queue[0].get_trajectory() + queue[0].set_status(1) + if canceled_trajectory: + rospy.sleep(0.5) + canceled_trajectory = False + client.send_goal(goal, done_cb=done_callback) + elif queue[0].get_status() == 1: + if 2 <= client.get_state() <= 8: + queue[0].set_status(2) + queue[0].set_action_status(client.get_state()) + elif queue[0].get_status() == 2: + queue[0].set_status(100) + cemetery.append(queue.pop(0)) + + if len(cemetery) > 0: + for trajectory_wrapper in cemetery: + if trajectory_wrapper.get_status() == 130: + cemetery.remove(trajectory_wrapper) + else: + trajectory_wrapper.set_status(trajectory_wrapper.get_status() + 1) + + msg = FanucTrajectoryConverterQueue() + msg.header.stamp = rospy.Time.now() + + for trajectory_wrapper in cemetery: + goal = FanucTrajectoryConverterQueueItem() + goal.received_time_stamp = trajectory_wrapper.get_trajectory().header.stamp + goal.status_in_queue = trajectory_wrapper.get_status() + goal.status_on_server = trajectory_wrapper.get_action_status() + msg.goals.append(goal) + + for trajectory_wrapper in queue: + goal = FanucTrajectoryConverterQueueItem() + goal.received_time_stamp = trajectory_wrapper.get_trajectory().header.stamp + goal.status_in_queue = trajectory_wrapper.get_status() + goal.status_on_server = trajectory_wrapper.get_action_status() + msg.goals.append(goal) + + queue_publisher.publish(msg) + + rospy.sleep(0.1) + + +if __name__ == '__main__': + main() diff --git a/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_write_single_io_converter_node.py b/ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_write_single_io_converter_node.py new file mode 100644 index 0000000..e69de29 diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/CMakeLists.txt b/ros1_noetic_core/fanuc_msg_translator_msgs/CMakeLists.txt new file mode 100644 index 0000000..abfa7db --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/CMakeLists.txt @@ -0,0 +1,213 @@ +cmake_minimum_required(VERSION 3.0.2) +project(fanuc_msg_translator_msgs) + +## 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 COMPONENTS + roscpp + rospy + std_msgs + message_generation +) + +## 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 + FanucTrajectoryConverterQueue.msg + FanucTrajectoryConverterQueueItem.msg + FanucTrajectoryConverterCancel.msg + FanucRobotStatus.msg + FanucRobotStatusTriState.msg + FanucPayloadNum.msg + FanucIOState.msg + FanucReadSingleIO.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 fanuc_msg_translator_msgs + CATKIN_DEPENDS message_runtime +# 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}/fanuc_msg_translator_msgs.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/fanuc_msg_translator_msgs_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_fanuc_msg_translator_msgs.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/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucIOState.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucIOState.msg new file mode 100644 index 0000000..1f17dce --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucIOState.msg @@ -0,0 +1,15 @@ +Header header + +# type : the type of the I/O to controll +# address : 1-base index +# value : 0 or 1 (for digital) + +uint32 type +uint32 address +uint32 value + +uint32 TYPE_DI=0 +uint32 TYPE_DO=1 +uint32 TYPE_RI=2 +uint32 TYPE_RO=3 +uint32 TYPE_FLAG=4 \ No newline at end of file diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg new file mode 100644 index 0000000..633426c --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg @@ -0,0 +1,3 @@ +Header header + +int16 payload_num \ No newline at end of file diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucReadSingleIO.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucReadSingleIO.msg new file mode 100644 index 0000000..7ebdfc5 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucReadSingleIO.msg @@ -0,0 +1,12 @@ +Header header + +# type : the type of the I/O to control +# address : 1-base index +uint32 type +uint32 address + +uint32 TYPE_DI=0 +uint32 TYPE_DO=1 +uint32 TYPE_RI=2 +uint32 TYPE_RO=3 +uint32 TYPE_FLAG=4 \ No newline at end of file diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg new file mode 100644 index 0000000..ad3a297 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg @@ -0,0 +1,32 @@ +# The RobotStatus message contains low level status information +# that is specific to an industrial robot controller + +# The header frame ID is not used +Header header + +# The robot mode captures the operating mode of the robot. When in +# manual, remote motion is not possible. +fanuc_msg_translator_msgs/FanucRobotStatusTriState tp_enabled + +# Estop status: True if robot is e-stopped. The drives are disabled +# and motion is not possible. The e-stop condition must be acknowledged +# and cleared before any motion can begin. +fanuc_msg_translator_msgs/FanucRobotStatusTriState e_stopped + +# Drive power status: True if drives are powered. Motion commands will +# automatically enable the drives if required. Drive power is not requred +# for possible motion +fanuc_msg_translator_msgs/FanucRobotStatusTriState drives_powered + +# Motion enabled: True if robot motion is possible. +fanuc_msg_translator_msgs/FanucRobotStatusTriState motion_possible + +# Motion status: True if robot is in motion, otherwise false +fanuc_msg_translator_msgs/FanucRobotStatusTriState in_motion + +# Error status: True if there is an error condition on the robot. Motion may +# or may not be affected (see motion_possible) +fanuc_msg_translator_msgs/FanucRobotStatusTriState in_error + +# Error code: Vendor specific error code (non zero indicates error) +int32 error_code \ No newline at end of file diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg new file mode 100644 index 0000000..6992e4c --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg @@ -0,0 +1,23 @@ + +# The tri-state captures boolean values with the additional state of unknown + +int8 val + +# enumerated values + +# Unknown or unavailable +int8 UNKNOWN=-1 + +# High state +int8 TRUE=1 +int8 ON=1 +int8 ENABLED=1 +int8 HIGH=1 +int8 CLOSED=1 + +# Low state +int8 FALSE=0 +int8 OFF=0 +int8 DISABLED=0 +int8 LOW=0 +int8 OPEN=0 diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg new file mode 100644 index 0000000..2b78cbd --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +time received_time_stamp diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg new file mode 100644 index 0000000..ea2d7e5 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +fanuc_msg_translator_msgs/FanucTrajectoryConverterQueueItem[] goals diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg new file mode 100644 index 0000000..809875e --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg @@ -0,0 +1,7 @@ +time received_time_stamp + +int16 status_in_queue + +int16 status_on_server + + diff --git a/ros1_noetic_core/fanuc_msg_translator_msgs/package.xml b/ros1_noetic_core/fanuc_msg_translator_msgs/package.xml new file mode 100644 index 0000000..1cb5df1 --- /dev/null +++ b/ros1_noetic_core/fanuc_msg_translator_msgs/package.xml @@ -0,0 +1,63 @@ + + + fanuc_msg_translator_msgs + 0.0.0 + The fanuc_msg_translator_msgs package + + + + + fanuc1 + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + rospy + rospy + rospy + std_msgs + + + + + + + + diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/__init__.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_connector.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_connector.py new file mode 100644 index 0000000..40c3fee --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_connector.py @@ -0,0 +1,92 @@ +import rclpy +from fanuc_msg_translator_msgs.msg import FanucTrajectoryConverterQueue, FanucTrajectoryConverterQueueItem, \ + FanucTrajectoryConverterCancel +from rclpy.node import Node +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_msgs.msg import Header, Int16 + + +class FanucControlPanelConnector(Node): + def __init__(self): + super().__init__("fanuc_control_panel_connector") + self.currentPosition = JointTrajectoryPoint() + self.trajectoryPublisher = self.create_publisher(JointTrajectory, + '/fanuc_1/bridge/follow_joint_trajectory/goal', 10) + self.trajectoryPublisher2 = self.create_publisher(JointTrajectory, '/fanuc_2/bridge/follow_joint_trajectory/goal', 10) + self.jointStateSubscription = self.create_subscription(JointState, '/fanuc_1/joint_states', + self.joint_states_callback, + 10) + self.goalSubscription = self.create_subscription(JointTrajectoryPoint, 'goal_point', self.goal_points_callback, + 10) + self.goalListSubscription = self.create_subscription(JointTrajectory, 'goal_point_list', + self.goal_points_list_callback, + 10) + self.cancelTrajectorySubscription = self.create_subscription(Int16, 'cancel_trajectory', + self.cancel_trajectory_callback, 10) + + self.queueSubscription = self.create_subscription(FanucTrajectoryConverterQueue, '/fanuc_1/bridge' + '/follow_joint_trajectory' + '/queue', + self.queue_callback, 10) + self.queue = FanucTrajectoryConverterQueue() + + self.cancelPublisher = self.create_publisher(FanucTrajectoryConverterCancel, '/fanuc_1/bridge/follow_joint_trajectory/cancel', 10) + + def send_trajectory(self, trajectory): + self.trajectoryPublisher.publish(trajectory) + self.trajectoryPublisher2.publish(trajectory) + self.get_logger().info('Published JointTrajectory message') + + def joint_states_callback(self, joint_states): + self.currentPosition.positions = joint_states.position + + def goal_points_callback(self, goal_points): + goal_points.time_from_start.sec = 1 + trajectory = JointTrajectory() + trajectory.header.frame_id = 'base_link' + trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'] + trajectory.points.append(self.currentPosition) + trajectory.points.append(goal_points) + + self.send_trajectory(trajectory) + + def goal_points_list_callback(self, goal_point_list): + trajectory = JointTrajectory() + trajectory.header = Header() + trajectory.header.frame_id = 'base_link' + trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'] + + trajectory.points = (goal_point_list.points) + trajectory.points.insert(0, self.currentPosition) + self.send_trajectory(trajectory) + + def cancel_trajectory_callback(self, cancel_trajectory): + try: + cancel = FanucTrajectoryConverterCancel() + + self.get_logger().info('Canceling trajectory with index {}'.format(cancel_trajectory.data)) + self.get_logger().info('Queue ' + str(self.queue.goals)) + queue_item = self.queue.goals[cancel_trajectory.data] + + cancel.received_time_stamp = queue_item.received_time_stamp + + self.cancelPublisher.publish(cancel) + except IndexError: + self.get_logger().info('IndexError: No item with index {} in queue'.format(cancel_trajectory.data)) + + def queue_callback(self, queue): + self.queue = queue + + +def main(args=None): + rclpy.init(args=args) + node = FanucControlPanelConnector() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_gui.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_gui.py new file mode 100644 index 0000000..6c602b6 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_gui.py @@ -0,0 +1,112 @@ +import rclpy +import math +from rclpy.node import Node +from std_msgs.msg import Int16 + +import tkinter as tk +from tkinter import messagebox +from fanuc_msg_translator_msgs.msg import FanucPayloadNum +from trajectory_msgs.msg import JointTrajectoryPoint + + +class FanucControlPanelGUI(Node): + + def __init__(self): + super().__init__('fanuc_control_panel_gui') + self.publisher_goal_point = self.create_publisher(JointTrajectoryPoint, 'goal_point', 10) + self.publisher_int = self.create_publisher(FanucPayloadNum, '/fanuc_1/bridge/set_payload_num/goal', 10) + self.publisher_cancel_trajectory = self.create_publisher(Int16, 'cancel_trajectory', 10) + + # Initialize the Tkinter GUI + self.root = tk.Tk() + self.root.title('ROS2 GUI') + + # First segment: 6 input fields and a button + self.segment1 = tk.Frame(self.root) + self.segment1.pack(pady=10) + + self.input_fields = [] + for i in range(6): + label = tk.Label(self.segment1, text=f'Joint {i+1}: ') + label.grid(row=i, column=0, padx=10, pady=5) + + input_field = tk.Entry(self.segment1) + input_field.grid(row=i, column=1, padx=10, pady=5) + self.input_fields.append(input_field) + + button_float = tk.Button(self.segment1, text='Send Joint Angles', command=self.send_goal_pose) + button_float.grid(row=6, column=0, columnspan=2, pady=5) + + # Second segment: 1 input field and a button + self.segment2 = tk.Frame(self.root) + self.segment2.pack(pady=10) + + label_cancel = tk.Label(self.segment2, text='Cancel Trajectory: ') + label_cancel.grid(row=0, column=0, padx=10, pady=5) + + self.input_field_cancel = tk.Entry(self.segment2) + self.input_field_cancel.grid(row=0, column=1, padx=10, pady=5) + + button_cancel = tk.Button(self.segment2, text='Cancel', command=self.send_cancel_trajectory) + button_cancel.grid(row=1, column=0, columnspan=2, pady=5) + + # Second segment: 1 input field and a button + self.segment3 = tk.Frame(self.root) + self.segment3.pack(pady=10) + + label_int = tk.Label(self.segment3, text='Payload Nr: ') + label_int.grid(row=0, column=0, padx=10, pady=5) + + self.input_field_int = tk.Entry(self.segment3) + self.input_field_int.grid(row=0, column=1, padx=10, pady=5) + + button_int = tk.Button(self.segment3, text='Send Payload Nr', command=self.send_payload_nr) + button_int.grid(row=1, column=0, columnspan=2, pady=5) + + # Start the ROS2 node + self.start() + + def send_goal_pose(self): + try: + values = [float(field.get())*(2*math.pi)/360 for field in self.input_fields] + msg = JointTrajectoryPoint() + msg.positions = values + self.publisher_goal_point.publish(msg) + self.get_logger().info(f'Goal position: {values}') + except ValueError: + messagebox.showerror('Error', 'Invalid float value') + + def send_payload_nr(self): + try: + value = int(self.input_field_int.get()) + fanuc_payload_num = FanucPayloadNum() + fanuc_payload_num.payload_num = value + self.publisher_int.publish(fanuc_payload_num) + self.get_logger().info(f'Payload set to Nr: {value}') + except ValueError: + messagebox.showerror('Error', 'Invalid integer value') + + def send_cancel_trajectory(self): + try: + value = int(self.input_field_cancel.get()) + msg = Int16() + msg.data = value + self.publisher_cancel_trajectory.publish(msg) + self.get_logger().info(f'Cancel trajectory: {value}') + except ValueError: + messagebox.showerror('Error', 'Invalid integer value') + + def start(self): + self.root.mainloop() + + +def main(args=None): + rclpy.init(args=args) + gui_node = FanucControlPanelGUI() + rclpy.spin(gui_node) + gui_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_status_viewer.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_status_viewer.py new file mode 100644 index 0000000..9ce71c5 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_status_viewer.py @@ -0,0 +1,97 @@ +import tkinter as tk +import rclpy +from rclpy.node import Node +from fanuc_msg_translator_msgs.msg import FanucRobotStatus, FanucRobotStatusTriState + + +class RobotStatusSubscriber(Node): + def __init__(self): + super().__init__('robot_status_subscriber') + self.subscription = self.create_subscription( + FanucRobotStatus, + '/fanuc_1/bridge/robot_status', + self.robot_status_callback, + 10 + ) + # Create the GUI window + self.window = tk.Tk() + self.window.title('Robot Status') + self.labels = [] + self.create_gui() + + + + + + + def robot_status_callback(self, msg): + self.get_logger().info("Values: " + str(msg)) + # Extract values from the FanucRobotStatus message + tp_enabled = msg.tp_enabled.val + e_stopped = msg.e_stopped.val + drives_powered = msg.drives_powered.val + motion_possible = msg.motion_possible.val + in_motion = msg.in_motion.val + in_error = msg.in_error.val + error_code = msg.error_code + + values = [tp_enabled, e_stopped, drives_powered, motion_possible, + in_motion, in_error, error_code] + + for label in self.labels: + label.config(text=str(values[self.labels.index(label)])) + if self.labels.index(label) < 5: + label.config(fg='green' if values[self.labels.index(label)] == 1 else 'red') + + self.window.update() + + + + + + + + + + def create_gui(self): + tk.Label(self.window, text='TP Enabled').grid(row=0, column=0, sticky='e') + tk.Label(self.window, text='E-Stopped').grid(row=1, column=0, sticky='e') + tk.Label(self.window, text='Drives Powered').grid(row=2, column=0, sticky='e') + tk.Label(self.window, text='Motion Possible').grid(row=3, column=0, sticky='e') + tk.Label(self.window, text='In Motion').grid(row=4, column=0, sticky='e') + tk.Label(self.window, text='In Error').grid(row=5, column=0, sticky='e') + tk.Label(self.window, text='Error Code').grid(row=6, column=0, sticky='e') + + self.labels.append(tk.Label(self.window, text='0')) + self.labels[0].grid(row=0, column=1, sticky='w') + self.labels.append(tk.Label(self.window, text='0')) + self.labels[1].grid(row=1, column=1, sticky='w') + self.labels.append(tk.Label(self.window, text='0')) + self.labels[2].grid(row=2, column=1, sticky='w') + self.labels.append(tk.Label(self.window, text='0')) + self.labels[3].grid(row=3, column=1, sticky='w') + self.labels.append(tk.Label(self.window, text='0')) + self.labels[4].grid(row=4, column=1, sticky='w') + self.labels.append(tk.Label(self.window, text='0')) + self.labels[5].grid(row=5, column=1, sticky='w') + self.labels.append(tk.Label(self.window, text='0')) + self.labels[6].grid(row=6, column=1, sticky='w') + + self.window.update() + + + + + + +def main(args=None): + rclpy.init(args=args) + subscriber = RobotStatusSubscriber() + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_trajectory_queue.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_trajectory_queue.py new file mode 100644 index 0000000..2e98cea --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_trajectory_queue.py @@ -0,0 +1,64 @@ +import rclpy +from rclpy.node import Node +from fanuc_msg_translator_msgs.msg import FanucTrajectoryConverterQueue, FanucTrajectoryConverterQueueItem +import tkinter as tk +from tkinter import ttk + + +class FanucGUI(Node): + + def __init__(self): + super().__init__('fanuc_gui') + self.subscription = self.create_subscription( + FanucTrajectoryConverterQueue, + '/fanuc_1/bridge/follow_joint_trajectory/queue', + self.queue_callback, + 10 + ) + + + self.root = tk.Tk() + self.treeview = ttk.Treeview(self.root) + self.treeview['columns'] = ('Nr', 'time_stamp_s', 'time_stamp_ns', 'status_in_queue', 'status_on_server') + + self.treeview.column('#0', width=0, stretch=tk.NO) + self.treeview.column('Nr', anchor=tk.CENTER, width=50) + self.treeview.column('time_stamp_s', anchor=tk.CENTER, width=150) + self.treeview.column('time_stamp_ns', anchor=tk.CENTER, width=150) + self.treeview.column('status_in_queue', anchor=tk.CENTER, width=150) + self.treeview.column('status_on_server', anchor=tk.CENTER, width=150) + + self.treeview.heading('#0', text='', anchor=tk.CENTER) + self.treeview.heading('Nr', text='Nr', anchor=tk.CENTER) + self.treeview.heading('time_stamp_s', text='Time Stamp [s]', anchor=tk.CENTER) + self.treeview.heading('time_stamp_ns', text='Time Stamp [ns]', anchor=tk.CENTER) + self.treeview.heading('status_in_queue', text='Status in Queue', anchor=tk.CENTER) + self.treeview.heading('status_on_server', text='Status on Server', anchor=tk.CENTER) + + self.treeview.pack() + + def queue_callback(self, msg): + self.treeview.delete(*self.treeview.get_children()) # Clear the table + + for item in msg.goals: + nr = msg.goals.index(item) + time_stamp_s = item.received_time_stamp.sec + time_stamp_ns = item.received_time_stamp.nanosec + status_in_queue = item.status_in_queue + status_on_server = item.status_on_server + + self.treeview.insert('', tk.END, text='', values=(nr, time_stamp_s, time_stamp_ns, status_in_queue, status_on_server)) + self.root.update() # Update the GUI + + + +def main(args=None): + rclpy.init(args=args) + fanuc_gui = FanucGUI() + rclpy.spin(fanuc_gui) + fanuc_gui.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/goal_point_generator.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/goal_point_generator.py new file mode 100644 index 0000000..893649a --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/goal_point_generator.py @@ -0,0 +1,45 @@ +import rclpy +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_msgs.msg import Header +import random + + +class RandomJointTrajectoryNode(Node): + + def __init__(self): + super().__init__('random_joint_trajectory_node') + self.publisher_ = self.create_publisher(JointTrajectoryPoint, '/goal_point', 10) + self.timer_ = self.create_timer(1.0, self.timer_callback) # Publish every 1 second + + def timer_callback(self): + joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] + + trajectory = JointTrajectory() + trajectory.header = Header() + trajectory.header.stamp = self.get_clock().now().to_msg() + + trajectory.joint_names = joint_names + + point = JointTrajectoryPoint() + point.time_from_start.sec = 0 + point.time_from_start.nanosec = 0 + + # Generate random positions for each joint + point.positions = [random.uniform(-3.14, 3.14) for _ in range(len(joint_names))] + + trajectory.points = [point] + + self.publisher_.publish(point) + + +def main(args=None): + rclpy.init(args=args) + node = RandomJointTrajectoryNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/my_first_node.py b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/my_first_node.py new file mode 100644 index 0000000..513dee2 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/my_first_node.py @@ -0,0 +1,27 @@ +import rclpy +from rclpy.node import Node + + +class myNode(Node): # Class that inherits from node + def __init__(self): + super().__init__("First_Node") # Initialise the node and give it the name which will be visable in ROS2 + self.create_timer(1.0, self.timer_callback) + + def timer_callback(self): + self.get_logger().info("Hello") + + +def main(args=None): + # Initalise ROS2 communications + rclpy.init(args=args) + + # Start the node + node = myNode() + rclpy.spin(node) + + # Shutdown ROS2 communication + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_basic_demo_gui/fanuc_control_panel/launch/control_panel.launch.py b/ros2_basic_demo_gui/fanuc_control_panel/launch/control_panel.launch.py new file mode 100644 index 0000000..5dddbbd --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/launch/control_panel.launch.py @@ -0,0 +1,26 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='fanuc_control_panel', + executable='fanuc_control_panel_connector', + name='fanuc_control_panel_connector' + ), + Node( + package='fanuc_control_panel', + executable='fanuc_control_panel_gui', + name='fanuc_control_panel_gui' + ), + Node( + package='fanuc_control_panel', + executable='fanuc_control_panel_status_viewer', + name='fanuc_control_panel_status_viewer' + ), + Node( + package='fanuc_control_panel', + executable='fanuc_control_panel_trajectory_queue', + name='fanuc_control_panel_trajectory_queue' + ) + ]) \ No newline at end of file diff --git a/ros2_basic_demo_gui/fanuc_control_panel/package.xml b/ros2_basic_demo_gui/fanuc_control_panel/package.xml new file mode 100644 index 0000000..a8c0115 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/package.xml @@ -0,0 +1,20 @@ + + + + fanuc_control_panel + 0.0.0 + TODO: Package description + christoph + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ros2launch + + + ament_python + + diff --git a/ros2_basic_demo_gui/fanuc_control_panel/resource/fanuc_control_panel b/ros2_basic_demo_gui/fanuc_control_panel/resource/fanuc_control_panel new file mode 100644 index 0000000..e69de29 diff --git a/ros2_basic_demo_gui/fanuc_control_panel/setup.cfg b/ros2_basic_demo_gui/fanuc_control_panel/setup.cfg new file mode 100644 index 0000000..b6561db --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fanuc_control_panel +[install] +install_scripts=$base/lib/fanuc_control_panel diff --git a/ros2_basic_demo_gui/fanuc_control_panel/setup.py b/ros2_basic_demo_gui/fanuc_control_panel/setup.py new file mode 100644 index 0000000..3903231 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/setup.py @@ -0,0 +1,32 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'fanuc_control_panel' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='christoph', + maintainer_email='christoph.andres@stud.unileoben.ac.at', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "fanuc_control_panel_connector = fanuc_control_panel.fanuc_control_panel_connector:main", + "fanuc_control_panel_gui = fanuc_control_panel.fanuc_control_panel_gui:main", + "fanuc_control_panel_status_viewer = fanuc_control_panel.fanuc_control_panel_status_viewer:main", + "fanuc_control_panel_trajectory_queue = fanuc_control_panel.fanuc_control_panel_trajectory_queue:main" + ], + }, +) diff --git a/ros2_basic_demo_gui/fanuc_control_panel/test/test_copyright.py b/ros2_basic_demo_gui/fanuc_control_panel/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/ros2_basic_demo_gui/fanuc_control_panel/test/test_flake8.py b/ros2_basic_demo_gui/fanuc_control_panel/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/ros2_basic_demo_gui/fanuc_control_panel/test/test_pep257.py b/ros2_basic_demo_gui/fanuc_control_panel/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_basic_demo_gui/fanuc_control_panel/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/__init__.py b/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/replay_manager.py b/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/replay_manager.py new file mode 100644 index 0000000..565fcfe --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/replay_manager.py @@ -0,0 +1,307 @@ +import time +import pymongo +from bson import ObjectId + +import rclpy +from builtin_interfaces.msg import Time +from rclpy.node import Node +from rosidl_runtime_py import convert +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from fanuc_web_connector_msgs.msg import TrajectoryDB, TrajectoryDBItem, RecordTrajectory, ReplayTrajectory +import json + + +class ReplayManager(Node): + def __init__(self): + super().__init__('replay_manager') + + self.mongo_client = pymongo.MongoClient('localhost', 27017) + self.mongo_db = self.mongo_client['ros_web_service_demo'] + self.mongo_collection = self.mongo_db['recorded_trajectories'] + + self.fanuc_1_joint_states = JointState() + self.fanuc_2_joint_states = JointState() + + self.fanuc_1_record_name = None + self.fanuc_2_record_name = None + + self.fanuc_1_record = False + self.fanuc_2_record = False + + self.fanuc_1_trajectory = JointTrajectory() + self.fanuc_2_trajectory = JointTrajectory() + + self.db_trajectories = [] + + self.pair_trajectory = None + + self.fanuc_1_waiting_for_fanuc_2 = False + self.fanuc_2_waiting_for_fanuc_1 = False + + self.timer_pair_replay_trajectory = None + self.timer_pair_replay_trajectory_counter = 0 + + self.timer_db_publisher = self.create_timer(1.0, self.db_publisher_callback) + + self.publisher_fanuc_1_goal_trajectory = self.create_publisher(JointTrajectory, '/fanuc_1/goal_trajectory', 10) + self.publisher_fanuc_2_goal_trajectory = self.create_publisher(JointTrajectory, '/fanuc_2/goal_trajectory', 10) + + self.subscription_fanuc_1_joint_states = self.create_subscription(JointState, '/fanuc_1/joint_states', + self.fanuc_1_joint_states_callback, + 10) + self.subscription_fanuc_2_joint_states = self.create_subscription(JointState, '/fanuc_2/joint_states', + self.fanuc_2_joint_states_callback, + 10) + + self.publisher_trajectory_db = self.create_publisher(TrajectoryDB, '/trajectory_db', 10) + + self.subscription_record_trajectory = self.create_subscription(RecordTrajectory, '/record_trajectory', + self.record_trajectory_callback, + 10) + + self.subscription_replay_trajectory = self.create_subscription(ReplayTrajectory, '/replay_trajectory', + self.replay_trajectory_callback, + 10) + + + + + self.get_logger().info('Node initialized') + + def db_publisher_callback(self): + cursor = self.mongo_collection.find({}, {"_id": 1, "trajectory_name": 1}) + self.db_trajectories = [] + + for document in cursor: + item = TrajectoryDBItem() + item.trajectory_name = document["trajectory_name"] + item.trajectory_id = str(document["_id"]) + self.db_trajectories.append(item) + + trajectory_db = TrajectoryDB() + trajectory_db.header.stamp = self.get_clock().now().to_msg() + trajectory_db.trajectories = self.db_trajectories + self.publisher_trajectory_db.publish(trajectory_db) + + + + def record_trajectory_callback(self, record_trajectory): + self.get_logger().info('RecordTrajectory message received') + if self.fanuc_1_record_name is None and self.fanuc_2_record_name is None and record_trajectory.record: + if record_trajectory.robot_name == "fanuc_1": + self.fanuc_1_record_name = record_trajectory.trajectory_name + self.fanuc_1_record = True + self.fanuc_1_trajectory = JointTrajectory() + self.fanuc_1_trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'] + + elif record_trajectory.robot_name == "fanuc_2": + self.fanuc_2_record_name = record_trajectory.trajectory_name + self.fanuc_2_record = True + self.fanuc_2_trajectory = JointTrajectory() + self.fanuc_2_trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'] + + elif record_trajectory.record is False: + if record_trajectory.robot_name == "fanuc_1": + self.fanuc_1_record = False + elif record_trajectory.robot_name == "fanuc_2": + self.fanuc_2_record = False + + def pair_replay_trajectory_callback(self): + if self.timer_pair_replay_trajectory_counter < 150: + self.timer_pair_replay_trajectory_counter += 1 + if self.pair_trajectory is not None: + if not self.fanuc_1_waiting_for_fanuc_2 and not self.fanuc_2_waiting_for_fanuc_1: + time.sleep(0.4) + self.publisher_fanuc_1_goal_trajectory.publish(self.pair_trajectory) + self.publisher_fanuc_2_goal_trajectory.publish(self.pair_trajectory) + self.get_logger().info('Trajectory published for fanuc_1 and fanuc_2') + self.pair_trajectory = None + self.timer_pair_replay_trajectory_counter = 0 + self.timer_pair_replay_trajectory.cancel() + + else: + self.get_logger().info('Trajectory not published for fanuc_1 and fanuc_2') + self.pair_trajectory = None + self.fanuc_1_waiting_for_fanuc_2 = False + self.fanuc_2_waiting_for_fanuc_1 = False + self.timer_pair_replay_trajectory_counter = 0 + self.timer_pair_replay_trajectory.cancel() + + def replay_trajectory_callback(self, replay_trajectory): + if len(replay_trajectory.robot_names) == 1: + if replay_trajectory.robot_names[0] == "fanuc_1": + self.get_logger().info('ReplayTrajectory message received') + self.get_logger().info('Replaying trajectory for fanuc_1') + + trajectory = self.load_trajectory_from_db(replay_trajectory.trajectory_id) + if trajectory is not None: + self.publisher_fanuc_1_goal_trajectory.publish(trajectory) + self.get_logger().info('Trajectory published for fanuc_1') + else: + self.get_logger().info('Trajectory not found in DB') + elif replay_trajectory.robot_names[0] == "fanuc_2": + self.get_logger().info('ReplayTrajectory message received') + self.get_logger().info('Replaying trajectory for fanuc_2') + + trajectory = self.load_trajectory_from_db(replay_trajectory.trajectory_id) + if trajectory is not None: + self.publisher_fanuc_2_goal_trajectory.publish(trajectory) + self.get_logger().info('Trajectory published for fanuc_2') + else: + self.get_logger().info('Trajectory not found in DB') + else: + if (replay_trajectory.robot_names[0] == "fanuc_1" and replay_trajectory.robot_names[1] == "fanuc_2") or \ + (replay_trajectory.robot_names[0] == "fanuc_2" and replay_trajectory.robot_names[1] == "fanuc_1"): + self.get_logger().info('ReplayTrajectory message received for pair') + pair_trajectory = self.load_trajectory_from_db(replay_trajectory.trajectory_id) + if pair_trajectory is not None: + temp_trajectory = JointTrajectory() + temp_trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'] + temp_trajectory.points.append(pair_trajectory.points[0]) + self.publisher_fanuc_1_goal_trajectory.publish(temp_trajectory) + self.publisher_fanuc_2_goal_trajectory.publish(temp_trajectory) + self.fanuc_1_waiting_for_fanuc_2 = True + self.fanuc_2_waiting_for_fanuc_1 = True + self.pair_trajectory = pair_trajectory + + self.timer_pair_replay_trajectory = self.create_timer(0.1, self.pair_replay_trajectory_callback) + + self.get_logger().info('Trajectory published for pair') + + + else: + self.get_logger().info('ReplayTrajectory message received') + self.get_logger().info('Robot names are not correct') + + def fanuc_1_joint_states_callback(self, joint_states): + self.get_logger().info('JointState message received') + self.fanuc_1_joint_states = joint_states + if self.fanuc_1_record_name is not None: + if self.fanuc_1_record: + joint_trajectory_point = JointTrajectoryPoint() + joint_trajectory_point.time_from_start.sec = joint_states.header.stamp.sec + joint_trajectory_point.time_from_start.nanosec = joint_states.header.stamp.nanosec + joint_trajectory_point.positions = joint_states.position + self.fanuc_1_trajectory.points.append(joint_trajectory_point) + else: + first_time_stamp = self.fanuc_1_trajectory.points[0].time_from_start + final_trajectory = JointTrajectory() + for point in self.fanuc_1_trajectory.points: + new_point = JointTrajectoryPoint() + new_point.positions = point.positions + + new_time_stamp = self.calculate_time_difference(first_time_stamp, point.time_from_start) + new_point.time_from_start.sec = new_time_stamp.sec + 1 + new_point.time_from_start.nanosec = new_time_stamp.nanosec + final_trajectory.points.append(new_point) + self.save_joint_trajectory(final_trajectory, (str(self.fanuc_1_record_name))) + self.fanuc_1_record_name = None + self.fanuc_1_trajectory = JointTrajectory() + elif self.pair_trajectory is not None: + self.get_logger().info('Publishing pair trajectory') + self.get_logger().info('Fanuc_1 waiting for Fanuc_2: ' + str(self.fanuc_1_waiting_for_fanuc_2)) + self.get_logger().info('Fanuc_2 waiting for Fanuc_1: ' + str(self.fanuc_2_waiting_for_fanuc_1)) + + if self.fanuc_2_waiting_for_fanuc_1 and self.confiuration_reached(joint_states, self.pair_trajectory.points[0]): + self.fanuc_2_waiting_for_fanuc_1 = False + + def fanuc_2_joint_states_callback(self, joint_states): + self.get_logger().info('JointState message received') + self.fanuc_2_joint_states = joint_states + if self.fanuc_2_record_name is not None: + if self.fanuc_2_record: + joint_trajectory_point = JointTrajectoryPoint() + joint_trajectory_point.time_from_start.sec = joint_states.header.stamp.sec + joint_trajectory_point.time_from_start.nanosec = joint_states.header.stamp.nanosec + joint_trajectory_point.positions = joint_states.position + self.fanuc_2_trajectory.points.append(joint_trajectory_point) + else: + first_time_stamp = self.fanuc_2_trajectory.points[0].time_from_start + final_trajectory = JointTrajectory() + for point in self.fanuc_2_trajectory.points: + new_point = JointTrajectoryPoint() + new_point.positions = point.positions + + new_time_stamp = self.calculate_time_difference(first_time_stamp, point.time_from_start) + new_point.time_from_start.sec = new_time_stamp.sec + 1 + new_point.time_from_start.nanosec = new_time_stamp.nanosec + final_trajectory.points.append(new_point) + self.save_joint_trajectory(final_trajectory, (str(self.fanuc_2_record_name))) + self.fanuc_2_record_name = None + self.fanuc_2_trajectory = JointTrajectory() + elif self.pair_trajectory is not None: + self.get_logger().info('Publishing pair trajectory') + + if self.fanuc_1_waiting_for_fanuc_2 and self.confiuration_reached(joint_states, self.pair_trajectory.points[0]): + self.fanuc_1_waiting_for_fanuc_2 = False + + def save_joint_trajectory(self, joint_trajectory, trajectory_name): + try: + serialized_states = [] + for state in joint_trajectory.points: + serialized_states.append(convert.message_to_ordereddict(state)) + + self.mongo_collection.insert_one({"trajectory_name": trajectory_name, "trajectory": serialized_states}) + + except Exception as e: + self.get_logger().error(f"Failed to save joint states to file: {str(e)}") + + def load_trajectory_from_db(self, trajectory_id): + try: + entry = self.mongo_collection.find_one({"_id": ObjectId(trajectory_id)}) + serialized_states = entry["trajectory"] + + trajectory = JointTrajectory() + trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory.header.frame_id = "base_link" + trajectory.joint_names = ["J1", "J2", "J3", "J4", "J5", "J6"] + + for serialized_state in serialized_states: + state = JointTrajectoryPoint() + state.positions = serialized_state["positions"] + state.time_from_start.sec = serialized_state["time_from_start"]["sec"] + state.time_from_start.nanosec = serialized_state["time_from_start"]["nanosec"] + trajectory.points.append(state) + + return trajectory + + except Exception as e: + self.get_logger().error(f"Failed to load joint states from file: {str(e)}") + return None + + + + def calculate_time_difference(self, zero_time_stamp, current_time): + # Convert the given times into milliseconds + time1_ms = (zero_time_stamp.sec * 1000) + (zero_time_stamp.nanosec // 1000000) + time2_ms = (current_time.sec * 1000) + (current_time.nanosec // 1000000) + + # Calculate the time difference in milliseconds + time_diff_ms = time2_ms - time1_ms + + # Convert the time difference back to seconds and nanoseconds + diff_time = Time() + + diff_time.sec = time_diff_ms // 1000 + diff_time.nanosec = (time_diff_ms % 1000) * 1000000 + + return diff_time + + def confiuration_reached(self, joint_states, target_joint_states): + for i in range(len(joint_states.position)): + if abs(joint_states.position[i] - target_joint_states.positions[i]) > 0.1: + return False + return True + + +def main(args=None): + rclpy.init(args=args) + node = ReplayManager() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/trajectory_composer.py b/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/trajectory_composer.py new file mode 100644 index 0000000..c61302d --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/trajectory_composer.py @@ -0,0 +1,53 @@ +import rclpy + +from rclpy.node import Node +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +class TrajectoryComposer(Node): + def __init__(self): + super().__init__("trajectory_composer") + self.currentConfiguration = JointTrajectoryPoint() + self.jointStateSubscription = self.create_subscription(JointState, 'joint_states', + self.joint_states_callback, + 10) + self.goalSubscription = self.create_subscription(JointTrajectory, 'goal_trajectory', + self.goal_trajectory_callback, + 10) + self.trajectoryGoalPublisher = self.create_publisher(JointTrajectory, + 'bridge/follow_joint_trajectory/goal', 10) + + def joint_states_callback(self, joint_states): + self.get_logger().info('JointState message received') + self.currentConfiguration.positions = joint_states.position + + def goal_trajectory_callback(self, goal_trajectory): + trajectory = JointTrajectory() + trajectory.header.frame_id = 'base_link' + trajectory.header.stamp = self.get_clock().now().to_msg() + trajectory.joint_names = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'] + + if goal_trajectory.points[0].time_from_start.sec == 0 and goal_trajectory.points[ + 0].time_from_start.nanosec == 0: + self.get_logger().info('Wrong') + trajectory.points = goal_trajectory.points + else: + self.get_logger().info('Right') + trajectory.points = goal_trajectory.points + trajectory.points.insert(0, self.currentConfiguration) + + self.trajectoryGoalPublisher.publish(trajectory) + self.get_logger().info('Published JointTrajectory message') + + +def main(args=None): + rclpy.init(args=args) + node = TrajectoryComposer() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/launch/web_connector.launch.py b/ros2_fanuc_web_connector/fanuc_web_connector/launch/web_connector.launch.py new file mode 100644 index 0000000..69d2d5d --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/launch/web_connector.launch.py @@ -0,0 +1,28 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='fanuc_web_connector', + executable='replay_manager', + name='replay_manager' + ), + Node( + package='fanuc_web_connector', + executable='trajectory_composer', + name='trajectory_composer', + namespace='fanuc_1' + ), + Node( + package='fanuc_web_connector', + executable='trajectory_composer', + name='trajectory_composer', + namespace='fanuc_2' + ), + Node( + package='rosbridge_server', + executable='rosbridge_websocket', + name='rosbridge_websocket' + ) + ]) \ No newline at end of file diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/package.xml b/ros2_fanuc_web_connector/fanuc_web_connector/package.xml new file mode 100644 index 0000000..94710ff --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/package.xml @@ -0,0 +1,20 @@ + + + + fanuc_web_connector + 0.0.0 + TODO: Package description + christoph + TODO: License declaration + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/resource/fanuc_web_connector b/ros2_fanuc_web_connector/fanuc_web_connector/resource/fanuc_web_connector new file mode 100644 index 0000000..e69de29 diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/setup.cfg b/ros2_fanuc_web_connector/fanuc_web_connector/setup.cfg new file mode 100644 index 0000000..de7b925 --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fanuc_web_connector +[install] +install_scripts=$base/lib/fanuc_web_connector diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/setup.py b/ros2_fanuc_web_connector/fanuc_web_connector/setup.py new file mode 100644 index 0000000..a422430 --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/setup.py @@ -0,0 +1,31 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'fanuc_web_connector' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='christoph', + maintainer_email='christoph.andres@stud.unileoben.ac.at', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "trajectory_composer = fanuc_web_connector.trajectory_composer:main", + "replay_manager = fanuc_web_connector.replay_manager:main" + ], + }, +) diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/test/test_copyright.py b/ros2_fanuc_web_connector/fanuc_web_connector/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/test/test_flake8.py b/ros2_fanuc_web_connector/fanuc_web_connector/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/ros2_fanuc_web_connector/fanuc_web_connector/test/test_pep257.py b/ros2_fanuc_web_connector/fanuc_web_connector/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_fanuc_web_connector/fanuc_web_connector/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/ros2_foxy_core/fanuc_crx_description/CMakeLists.txt b/ros2_foxy_core/fanuc_crx_description/CMakeLists.txt new file mode 100644 index 0000000..164f74d --- /dev/null +++ b/ros2_foxy_core/fanuc_crx_description/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.10.2) +project(fanuc_crx_description) +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY meshes launch robots DESTINATION share/${PROJECT_NAME}) diff --git a/ros2_foxy_core/fanuc_crx_description/launch/fanuc_1.launch.py b/ros2_foxy_core/fanuc_crx_description/launch/fanuc_1.launch.py new file mode 100644 index 0000000..b78f736 --- /dev/null +++ b/ros2_foxy_core/fanuc_crx_description/launch/fanuc_1.launch.py @@ -0,0 +1,95 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +namespace = 'fanuc_1' + + +def generate_launch_description(): + # Set the path to this package. + pkg_share = FindPackageShare(package='fanuc_crx_description').find('fanuc_crx_description') + + # Set the path to the URDF file + urdf_model_path = os.path.join(pkg_share, 'robots/crx10ia.xacro') + + position_x = LaunchConfiguration('x') + position_y = LaunchConfiguration('y') + position_z = LaunchConfiguration('z') + orientation_roll = LaunchConfiguration('roll') + orientation_pitch = LaunchConfiguration('pitch') + orientation_yaw = LaunchConfiguration('yaw') + + declare_x_position = DeclareLaunchArgument( + name='x', + default_value="0", + description='Robot position in x direction' + ) + + declare_y_position = DeclareLaunchArgument( + name='y', + default_value="0", + description='Robot position in y direction' + ) + + declare_z_position = DeclareLaunchArgument( + name='z', + default_value="0", + description='Robot position in z direction' + ) + + declare_roll_orientation = DeclareLaunchArgument( + name='roll', + default_value="0", + description='Robot orientation in roll' + ) + + declare_pitch_orientation = DeclareLaunchArgument( + name='pitch', + default_value="0", + description='Robot orientation in pitch' + ) + + declare_yaw_orientation = DeclareLaunchArgument( + name='yaw', + default_value="0", + description='Robot orientation in yaw' + ) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + namespace=namespace, + executable='robot_state_publisher', + parameters=[{'frame_prefix': namespace + '/', + 'robot_description': Command(['xacro ', urdf_model_path])}], + arguments=[urdf_model_path] + ) + + start_static_transform_publisher = Node(package="tf2_ros", + namespace=namespace, + executable="static_transform_publisher", + name="static_transform_publisher", + arguments=[position_x, + position_y, + position_z, + orientation_roll, + orientation_pitch, + orientation_yaw, + "map", + namespace + "/base_link"]) + + ld = LaunchDescription() + + ld.add_action(declare_x_position) + ld.add_action(declare_y_position) + ld.add_action(declare_z_position) + ld.add_action(declare_roll_orientation) + ld.add_action(declare_pitch_orientation) + ld.add_action(declare_yaw_orientation) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_static_transform_publisher) + + return ld diff --git a/ros2_foxy_core/fanuc_crx_description/launch/fanuc_2.launch.py b/ros2_foxy_core/fanuc_crx_description/launch/fanuc_2.launch.py new file mode 100644 index 0000000..7e5ffb7 --- /dev/null +++ b/ros2_foxy_core/fanuc_crx_description/launch/fanuc_2.launch.py @@ -0,0 +1,95 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +namespace = 'fanuc_2' + + +def generate_launch_description(): + # Set the path to this package. + pkg_share = FindPackageShare(package='fanuc_crx_description').find('fanuc_crx_description') + + # Set the path to the URDF file + urdf_model_path = os.path.join(pkg_share, 'robots/crx10ia.xacro') + + position_x = LaunchConfiguration('x') + position_y = LaunchConfiguration('y') + position_z = LaunchConfiguration('z') + orientation_roll = LaunchConfiguration('roll') + orientation_pitch = LaunchConfiguration('pitch') + orientation_yaw = LaunchConfiguration('yaw') + + declare_x_position = DeclareLaunchArgument( + name='x', + default_value="0", + description='Robot position in x direction' + ) + + declare_y_position = DeclareLaunchArgument( + name='y', + default_value="0", + description='Robot position in y direction' + ) + + declare_z_position = DeclareLaunchArgument( + name='z', + default_value="0", + description='Robot position in z direction' + ) + + declare_roll_orientation = DeclareLaunchArgument( + name='roll', + default_value="0", + description='Robot orientation in roll' + ) + + declare_pitch_orientation = DeclareLaunchArgument( + name='pitch', + default_value="0", + description='Robot orientation in pitch' + ) + + declare_yaw_orientation = DeclareLaunchArgument( + name='yaw', + default_value="0", + description='Robot orientation in yaw' + ) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + namespace=namespace, + executable='robot_state_publisher', + parameters=[{'frame_prefix': namespace + '/', + 'robot_description': Command(['xacro ', urdf_model_path])}], + arguments=[urdf_model_path] + ) + + start_static_transform_publisher = Node(package="tf2_ros", + namespace=namespace, + executable="static_transform_publisher", + name="static_transform_publisher", + arguments=[position_x, + position_y, + position_z, + orientation_roll, + orientation_pitch, + orientation_yaw, + "map", + namespace + "/base_link"]) + + ld = LaunchDescription() + + ld.add_action(declare_x_position) + ld.add_action(declare_y_position) + ld.add_action(declare_z_position) + ld.add_action(declare_roll_orientation) + ld.add_action(declare_pitch_orientation) + ld.add_action(declare_yaw_orientation) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_static_transform_publisher) + + return ld diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/base.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/base.stl new file mode 100644 index 0000000..4947de0 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/base.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/j1.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j1.stl new file mode 100644 index 0000000..19ee82a Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j1.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/j2.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j2.stl new file mode 100644 index 0000000..863dab5 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j2.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/j3.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j3.stl new file mode 100644 index 0000000..5e49acf Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j3.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/j4.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j4.stl new file mode 100644 index 0000000..1341136 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j4.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/j5.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j5.stl new file mode 100644 index 0000000..ccd148e Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j5.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/collision/j6.stl b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j6.stl new file mode 100644 index 0000000..8604711 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/collision/j6.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/base.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/base.stl new file mode 100644 index 0000000..9ecb8a4 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/base.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/j1.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j1.stl new file mode 100644 index 0000000..066ad7e Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j1.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/j2.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j2.stl new file mode 100644 index 0000000..57897e2 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j2.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/j3.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j3.stl new file mode 100644 index 0000000..cfcd596 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j3.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/j4.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j4.stl new file mode 100644 index 0000000..c9a9f41 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j4.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/j5.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j5.stl new file mode 100644 index 0000000..133e431 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j5.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/meshes/visual/j6.stl b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j6.stl new file mode 100644 index 0000000..b015799 Binary files /dev/null and b/ros2_foxy_core/fanuc_crx_description/meshes/visual/j6.stl differ diff --git a/ros2_foxy_core/fanuc_crx_description/package.xml b/ros2_foxy_core/fanuc_crx_description/package.xml new file mode 100644 index 0000000..fe1a29c --- /dev/null +++ b/ros2_foxy_core/fanuc_crx_description/package.xml @@ -0,0 +1,18 @@ + + + fanuc_crx_description + 2.0.6 + TF and robot description for CRX-10iA + + Christoph Andres + + + BSD + + + ament_cmake + + + ament_cmake + + diff --git a/ros2_foxy_core/fanuc_crx_description/robots/crx10ia.xacro b/ros2_foxy_core/fanuc_crx_description/robots/crx10ia.xacro new file mode 100644 index 0000000..11ccbd3 --- /dev/null +++ b/ros2_foxy_core/fanuc_crx_description/robots/crx10ia.xacro @@ -0,0 +1,183 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/CMakeLists.txt b/ros2_foxy_core/fanuc_msg_translator_msgs/CMakeLists.txt new file mode 100644 index 0000000..a1782db --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(fanuc_msg_translator_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/FanucTrajectoryConverterQueue.msg" + "msg/FanucTrajectoryConverterQueueItem.msg" + "msg/FanucTrajectoryConverterCancel.msg" + "msg/FanucRobotStatus.msg" + "msg/FanucRobotStatusTriState.msg" + "msg/FanucPayloadNum.msg" + DEPENDENCIES std_msgs builtin_interfaces # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg +) + +ament_package() diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg new file mode 100644 index 0000000..245b1de --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +int16 payload_num diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg new file mode 100644 index 0000000..7b1c9e1 --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg @@ -0,0 +1,32 @@ +# The RobotStatus message contains low level status information +# that is specific to an industrial robot controller + +# The header frame ID is not used +std_msgs/Header header + +# The robot mode captures the operating mode of the robot. When in +# manual, remote motion is not possible. +fanuc_msg_translator_msgs/FanucRobotStatusTriState tp_enabled + +# Estop status: True if robot is e-stopped. The drives are disabled +# and motion is not possible. The e-stop condition must be acknowledged +# and cleared before any motion can begin. +fanuc_msg_translator_msgs/FanucRobotStatusTriState e_stopped + +# Drive power status: True if drives are powered. Motion commands will +# automatically enable the drives if required. Drive power is not requred +# for possible motion +fanuc_msg_translator_msgs/FanucRobotStatusTriState drives_powered + +# Motion enabled: True if robot motion is possible. +fanuc_msg_translator_msgs/FanucRobotStatusTriState motion_possible + +# Motion status: True if robot is in motion, otherwise false +fanuc_msg_translator_msgs/FanucRobotStatusTriState in_motion + +# Error status: True if there is an error condition on the robot. Motion may +# or may not be affected (see motion_possible) +fanuc_msg_translator_msgs/FanucRobotStatusTriState in_error + +# Error code: Vendor specific error code (non zero indicates error) +int32 error_code \ No newline at end of file diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg new file mode 100644 index 0000000..6992e4c --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg @@ -0,0 +1,23 @@ + +# The tri-state captures boolean values with the additional state of unknown + +int8 val + +# enumerated values + +# Unknown or unavailable +int8 UNKNOWN=-1 + +# High state +int8 TRUE=1 +int8 ON=1 +int8 ENABLED=1 +int8 HIGH=1 +int8 CLOSED=1 + +# Low state +int8 FALSE=0 +int8 OFF=0 +int8 DISABLED=0 +int8 LOW=0 +int8 OPEN=0 diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg new file mode 100644 index 0000000..278b1b3 --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +builtin_interfaces/Time received_time_stamp diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg new file mode 100644 index 0000000..ea2d7e5 --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +fanuc_msg_translator_msgs/FanucTrajectoryConverterQueueItem[] goals diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg new file mode 100644 index 0000000..6861922 --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg @@ -0,0 +1,7 @@ +builtin_interfaces/Time received_time_stamp + +int16 status_in_queue + +int16 status_on_server + + diff --git a/ros2_foxy_core/fanuc_msg_translator_msgs/package.xml b/ros2_foxy_core/fanuc_msg_translator_msgs/package.xml new file mode 100644 index 0000000..b455ba7 --- /dev/null +++ b/ros2_foxy_core/fanuc_msg_translator_msgs/package.xml @@ -0,0 +1,27 @@ + + + + fanuc_msg_translator_msgs + 0.0.0 + TODO: Package description + fanuc1 + TODO: License declaration + + ament_cmake + + std_msgs + builtin_interfaces + + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_humble_crx_description/fanuc_crx_description/CMakeLists.txt b/ros2_humble_crx_description/fanuc_crx_description/CMakeLists.txt new file mode 100644 index 0000000..02af2af --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.10.2) +project(fanuc_crx_description) +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY meshes urdf launch robots DESTINATION share/${PROJECT_NAME}) diff --git a/ros2_humble_crx_description/fanuc_crx_description/README.md b/ros2_humble_crx_description/fanuc_crx_description/README.md new file mode 100644 index 0000000..55f3354 --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/README.md @@ -0,0 +1,5 @@ +# panda_description + +The URDF model and meshes contained in this package were copied from the frankaemika `franka_ros` package and adapted for use with `moveit_resources`. + +All imported files were released under the Apache-2.0 license. diff --git a/ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_1.launch.py b/ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_1.launch.py new file mode 100644 index 0000000..f4d59ce --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_1.launch.py @@ -0,0 +1,93 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +namespace = 'fanuc_1' + + +def generate_launch_description(): + # Set the path to this package. + pkg_share = FindPackageShare(package='fanuc_crx_description').find('fanuc_crx_description') + + # Set the path to the URDF file + urdf_model_path = os.path.join(pkg_share, 'robots/crx10ia.xacro') + + position_x = LaunchConfiguration('x') + position_y = LaunchConfiguration('y') + position_z = LaunchConfiguration('z') + orientation_roll = LaunchConfiguration('roll') + orientation_pitch = LaunchConfiguration('pitch') + orientation_yaw = LaunchConfiguration('yaw') + + declare_x_position = DeclareLaunchArgument( + name='x', + default_value="0", + description='Robot position in x direction' + ) + + declare_y_position = DeclareLaunchArgument( + name='y', + default_value="0", + description='Robot position in y direction' + ) + + declare_z_position = DeclareLaunchArgument( + name='z', + default_value="0", + description='Robot position in z direction' + ) + + declare_roll_orientation = DeclareLaunchArgument( + name='roll', + default_value="0", + description='Robot orientation in roll' + ) + + declare_pitch_orientation = DeclareLaunchArgument( + name='pitch', + default_value="0", + description='Robot orientation in pitch' + ) + + declare_yaw_orientation = DeclareLaunchArgument( + name='yaw', + default_value="0", + description='Robot orientation in yaw' + ) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + namespace=namespace, + executable='robot_state_publisher', + parameters=[{'frame_prefix': namespace + '/', + 'robot_description': Command(['xacro ', urdf_model_path])}], + arguments=[urdf_model_path] + ) + + start_static_transform_publisher = Node(package="tf2_ros", + executable="static_transform_publisher", + arguments=[position_x, + position_y, + position_z, + orientation_roll, + orientation_pitch, + orientation_yaw, + "map", + namespace + "/base_link"]) + + ld = LaunchDescription() + + ld.add_action(declare_x_position) + ld.add_action(declare_y_position) + ld.add_action(declare_z_position) + ld.add_action(declare_roll_orientation) + ld.add_action(declare_pitch_orientation) + ld.add_action(declare_yaw_orientation) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_static_transform_publisher) + + return ld diff --git a/ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_2.launch.py b/ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_2.launch.py new file mode 100644 index 0000000..79ec522 --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_2.launch.py @@ -0,0 +1,93 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +namespace = 'fanuc_2' + + +def generate_launch_description(): + # Set the path to this package. + pkg_share = FindPackageShare(package='fanuc_crx_description').find('fanuc_crx_description') + + # Set the path to the URDF file + urdf_model_path = os.path.join(pkg_share, 'robots/crx10ia.xacro') + + position_x = LaunchConfiguration('x') + position_y = LaunchConfiguration('y') + position_z = LaunchConfiguration('z') + orientation_roll = LaunchConfiguration('roll') + orientation_pitch = LaunchConfiguration('pitch') + orientation_yaw = LaunchConfiguration('yaw') + + declare_x_position = DeclareLaunchArgument( + name='x', + default_value="0", + description='Robot position in x direction' + ) + + declare_y_position = DeclareLaunchArgument( + name='y', + default_value="0", + description='Robot position in y direction' + ) + + declare_z_position = DeclareLaunchArgument( + name='z', + default_value="0", + description='Robot position in z direction' + ) + + declare_roll_orientation = DeclareLaunchArgument( + name='roll', + default_value="0", + description='Robot orientation in roll' + ) + + declare_pitch_orientation = DeclareLaunchArgument( + name='pitch', + default_value="0", + description='Robot orientation in pitch' + ) + + declare_yaw_orientation = DeclareLaunchArgument( + name='yaw', + default_value="0", + description='Robot orientation in yaw' + ) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + namespace=namespace, + executable='robot_state_publisher', + parameters=[{'frame_prefix': namespace + '/', + 'robot_description': Command(['xacro ', urdf_model_path])}], + arguments=[urdf_model_path] + ) + + start_static_transform_publisher = Node(package="tf2_ros", + executable="static_transform_publisher", + arguments=[position_x, + position_y, + position_z, + orientation_roll, + orientation_pitch, + orientation_yaw, + "map", + namespace + "/base_link"]) + + ld = LaunchDescription() + + ld.add_action(declare_x_position) + ld.add_action(declare_y_position) + ld.add_action(declare_z_position) + ld.add_action(declare_roll_orientation) + ld.add_action(declare_pitch_orientation) + ld.add_action(declare_yaw_orientation) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(start_static_transform_publisher) + + return ld diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/base.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/base.stl new file mode 100644 index 0000000..4947de0 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/base.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j1.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j1.stl new file mode 100644 index 0000000..19ee82a Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j1.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j2.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j2.stl new file mode 100644 index 0000000..863dab5 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j2.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j3.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j3.stl new file mode 100644 index 0000000..5e49acf Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j3.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j4.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j4.stl new file mode 100644 index 0000000..1341136 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j4.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j5.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j5.stl new file mode 100644 index 0000000..ccd148e Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j5.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j6.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j6.stl new file mode 100644 index 0000000..8604711 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j6.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/base.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/base.stl new file mode 100644 index 0000000..9ecb8a4 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/base.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j1.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j1.stl new file mode 100644 index 0000000..066ad7e Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j1.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j2.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j2.stl new file mode 100644 index 0000000..57897e2 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j2.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j3.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j3.stl new file mode 100644 index 0000000..cfcd596 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j3.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j4.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j4.stl new file mode 100644 index 0000000..c9a9f41 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j4.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j5.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j5.stl new file mode 100644 index 0000000..133e431 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j5.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j6.stl b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j6.stl new file mode 100644 index 0000000..b015799 Binary files /dev/null and b/ros2_humble_crx_description/fanuc_crx_description/meshes/visual/j6.stl differ diff --git a/ros2_humble_crx_description/fanuc_crx_description/package.xml b/ros2_humble_crx_description/fanuc_crx_description/package.xml new file mode 100644 index 0000000..394b2cf --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/package.xml @@ -0,0 +1,17 @@ + + + fanuc_crx_description + 2.0.6 + TF and robot description for CRX-10iA + + Christoph Andres + + + BSD + + ament_cmake + + + ament_cmake + + diff --git a/ros2_humble_crx_description/fanuc_crx_description/robots/crx10ia.xacro b/ros2_humble_crx_description/fanuc_crx_description/robots/crx10ia.xacro new file mode 100644 index 0000000..40ab40d --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/robots/crx10ia.xacro @@ -0,0 +1,5 @@ + + + + + diff --git a/ros2_humble_crx_description/fanuc_crx_description/urdf/crx10ia.urdf.xacro b/ros2_humble_crx_description/fanuc_crx_description/urdf/crx10ia.urdf.xacro new file mode 100644 index 0000000..b17a946 --- /dev/null +++ b/ros2_humble_crx_description/fanuc_crx_description/urdf/crx10ia.urdf.xacro @@ -0,0 +1,233 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +