Niko/Fanuc_ROS1_ROS2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
Niko Feith 2023-08-30 15:46:39 +02:00
commit c345904031
114 changed files with 3905 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.idea/.gitignore vendored Normal file
View File

@ -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

10
.idea/Fanuc_ROS1_ROS2.iml Normal file
View File

@ -0,0 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$">
<excludeFolder url="file://$MODULE_DIR$/venv" />
</content>
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
</module>

6
.idea/inspectionProfiles/profiles_settings.xml Normal file
View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

4
.idea/misc.xml Normal file
View File

@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10 (Fanuc_ROS1_ROS2)" project-jdk-type="Python SDK" />
</project>

8
.idea/modules.xml Normal file
View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/Fanuc_ROS1_ROS2.iml" filepath="$PROJECT_DIR$/.idea/Fanuc_ROS1_ROS2.iml" />
</modules>
</component>
</project>

443
README.md Normal file
View File

@ -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 <name of ros1 overlay ws>
cd <name of ros1 overlay ws>
mkdir src
catkin_make
```
In another bash terminal:
```
source /path/to/foxy/setup.bash
cd ~
mkdir <name of ros2 overlay ws>
cd <name of ros2 overlay ws>
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 <name of the python file>
```
### 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 <name of the workspace folder>
cd <name of the workspace folder>
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 <robot_name_space>/joint_states topic.
### Robot Status
The current robot status can be be received from the <robot_name_space>/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 <robot_name_space>/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 <robot_name_space>/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 <robot_name_space>/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 <robot_name_space>/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 <robot_name_space>/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 <robot_name_space>/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_name_space>/robot_description
+ TF Prefix: <robot_name_space>
![Alt text](/imgs/rviz.png)

1
fanuc_licenses/license_nuc1.data Normal file
View File

@ -0,0 +1 @@
Dejk049IFca6vvB5Or3aDFH5/kVhdjBqTA0kuZKs0JW0ZwWoOypgXECZxtYc3w7Sc71eRa1iiMyxOPtDDyNusxLCjVdDmecgyi+K0+6emuuI3WkwU56wzxSLtlq45/8Wnz6Omix3066/eXCLv8pVnjPrXmvYYYt3CSKWSimobCPwVBga0+a0xjcIc9O47JAuMoxbUi7g20fJl1UXtUQ6eIeWEfpZ81prEtsdyaSvZKMMxeuOUdn3SiYtLe04/9E6fQUZ+qCBpKaLuobjf/SajQcgpaq+X+9U7m4eQa0zQnBZzpxHj3qbL9HJMc4Bp0wuq3lL+PhZd3jZBXmAHSl0Zg==

1
fanuc_licenses/license_nuc2.data Normal file
View File

@ -0,0 +1 @@
Nd2ivgRlJZETuI/xfa/Li9tkNQ4mrfDjciAK6Tz932YrnTWo2lAbLX/G+8Xl5aAGVnarySlX3deJd1ZYegGuFEmgIT2hXDSIFSSr9aWOs3KP++cpsZIDG1AVsdgvT6DCPJ2gth0u49H5Fl2ICfxqL0WoRGKh1HxWAxFyiD6kNlhQFLK4Zp8XkP8tH9a0fiImav/kgm3PEUCF1gwg2TBPmkWcNv8RpAvg/R67fgRh+uouq1rkU8vdMHbADHoCIWZ8j032ir5jvzvmJoBHiebRhWrSvVd9jqYRTztwvK7EpsSdcLb9ZPqgRwNxVzN3EJcooH0gDU82HuNE1j7GPMRH1w==

BIN
imgs/landscape_full_interface_ros1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 82 KiB

BIN
imgs/landscape_full_interface_ros2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 69 KiB

BIN
imgs/operation_mode_one_nuc.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 35 KiB

BIN
imgs/operation_mode_split.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 74 KiB

BIN
imgs/operation_mode_two_nuc.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 39 KiB

BIN
imgs/rviz.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 156 KiB

205
ros1_noetic_core/fanuc_msg_translator/CMakeLists.txt Normal file
View File

@ -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)

59
ros1_noetic_core/fanuc_msg_translator/config/params_bridge.yaml Normal file
View File

@ -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

3
ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_1.yaml Normal file
View File

@ -0,0 +1,3 @@
robot_ip: "192.168.1.201"
robot_port: "502"

3
ros1_noetic_core/fanuc_msg_translator/config/params_fanuc_2.yaml Normal file
View File

@ -0,0 +1,3 @@
robot_ip: "192.168.1.206"
robot_port: "502"

5
ros1_noetic_core/fanuc_msg_translator/launch/fanuc_1.launch Normal file
View File

@ -0,0 +1,5 @@
<?xml version="1.0"?>
<launch>
<rosparam command="load" file="$(find fanuc_msg_translator)/config/params_bridge.yaml"/>
<include file="$(find fanuc_msg_translator)/launch/fanuc_msg_translator_fanuc_1.launch"/>
</launch>

5
ros1_noetic_core/fanuc_msg_translator/launch/fanuc_2.launch Normal file
View File

@ -0,0 +1,5 @@
<?xml version="1.0"?>
<launch>
<rosparam command="load" file="$(find fanuc_msg_translator)/config/params_bridge.yaml"/>
<include file="$(find fanuc_msg_translator)/launch/fanuc_msg_translator_fanuc_2.launch"/>
</launch>

64
ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_1.launch Normal file
View File

@ -0,0 +1,64 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="fanuc_1"/>
<group ns="$(arg namespace)">
<arg name="robot_type" default="CRX-10iA"/>
<arg name="license" default="$(env FANUC_LICENSE)"/>
<arg name="create_license" default="false"/>
<arg name="fast_joints_hz" default="false"/>
<arg name="output" default="screen"/>
<rosparam ns="interface" command="load" file="$(find fanuc_msg_translator)/config/params_fanuc_1.yaml"/>
<node pkg="fanuc_ros_driver" name="interface" type="fanuc_interface_node" output="$(arg output)">
<param name="robot_type" value="$(arg robot_type)" type="str"/>
<param name="license_path" value="$(arg license)" type="str"/>
<param name="create_license" value="$(arg create_license)" type="bool"/>
<param name="fast_joints_hz" value="$(arg fast_joints_hz)" type="bool"/>
</node>
<node pkg="fanuc_msg_translator" type="fanuc_trajectory_converter_node.py" name="trajectory_converter" output="screen" />
<node pkg="fanuc_msg_translator" type="fanuc_payload_converter_node.py" name="payload_converter" output="screen" />
<node pkg="fanuc_msg_translator" type="fanuc_robot_status_converter_node.py" name="robot_status_converter" output="screen"/>
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
<param if="true" name="$(arg robot_description)" command="xacro --inorder '$(find crx_description)/robots/crx10ia.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find crx10ia_moveit_config)/config/crx10ia.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find crx10ia_moveit_config)/config/joint_limits.yaml"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find crx10ia_moveit_config)/config/kinematics.yaml"/>
</group>
<param name="tf_prefix" value="$(arg namespace)" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
<node pkg="tf" type="static_transform_publisher" name="stf_pub" args="0 -1 0 0 0 0 /map /$(arg namespace)/base_link 10" />
</group>
</launch>

64
ros1_noetic_core/fanuc_msg_translator/launch/fanuc_msg_translator_fanuc_2.launch Normal file
View File

@ -0,0 +1,64 @@
<?xml version="1.0"?>
<launch>
<arg name="namespace" default="fanuc_2"/>
<group ns="$(arg namespace)">
<arg name="robot_type" default="CRX-10iA"/>
<arg name="license" default="$(env FANUC_LICENSE)"/>
<arg name="create_license" default="false"/>
<arg name="fast_joints_hz" default="false"/>
<arg name="output" default="screen"/>
<rosparam ns="interface" command="load" file="$(find fanuc_msg_translator)/config/params_fanuc_2.yaml"/>
<node pkg="fanuc_ros_driver" name="interface" type="fanuc_interface_node" output="$(arg output)">
<param name="robot_type" value="$(arg robot_type)" type="str"/>
<param name="license_path" value="$(arg license)" type="str"/>
<param name="create_license" value="$(arg create_license)" type="bool"/>
<param name="fast_joints_hz" value="$(arg fast_joints_hz)" type="bool"/>
</node>
<node pkg="fanuc_msg_translator" type="fanuc_trajectory_converter_node.py" name="trajectory_converter" output="screen" />
<node pkg="fanuc_msg_translator" type="fanuc_payload_converter_node.py" name="payload_converter" output="screen" />
<node pkg="fanuc_msg_translator" type="fanuc_robot_status_converter_node.py" name="robot_status_converter" output="screen"/>
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
<param if="true" name="$(arg robot_description)" command="xacro --inorder '$(find crx_description)/robots/crx10ia.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find crx10ia_moveit_config)/config/crx10ia.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find crx10ia_moveit_config)/config/joint_limits.yaml"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find crx10ia_moveit_config)/config/kinematics.yaml"/>
</group>
<param name="tf_prefix" value="$(arg namespace)" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
<node pkg="tf" type="static_transform_publisher" name="stf_pub" args="0 1 0 0 0 0 /map /$(arg namespace)/base_link 10" />
</group>
</launch>

6
ros1_noetic_core/fanuc_msg_translator/launch/fanuc_pair.launch Normal file
View File

@ -0,0 +1,6 @@
<?xml version="1.0"?>
<launch>
<rosparam command="load" file="$(find fanuc_msg_translator)/config/params_bridge.yaml"/>
<include file="$(find fanuc_msg_translator)/launch/fanuc_msg_translator_fanuc_1.launch"/>
<include file="$(find fanuc_msg_translator)/launch/fanuc_msg_translator_fanuc_2.launch"/>
</launch>

68
ros1_noetic_core/fanuc_msg_translator/package.xml Normal file
View File

@ -0,0 +1,68 @@
<?xml version="1.0"?>
<package format="2">
<name>fanuc_msg_translator</name>
<version>0.0.0</version>
<description>The fanuc_msg_translator package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="fanuc1@todo.todo">fanuc1</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/fanuc_msg_translator</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>fanuc_msgs</exec_depend>
<exec_depend>industrial_msgs</exec_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

38
ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_joint_states_converter_node.py Normal file
View File

@ -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()

101
ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_payload_converter_node.py Normal file
View File

@ -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()

51
ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_read_single_io_converter_node.py Normal file
View File

@ -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()

57
ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_robot_status_converter_node.py Normal file
View File

@ -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()

230
ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_trajectory_converter_node.py Normal file
View File

@ -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()

0
ros1_noetic_core/fanuc_msg_translator/scripts/fanuc_write_single_io_converter_node.py Normal file
View File

213
ros1_noetic_core/fanuc_msg_translator_msgs/CMakeLists.txt Normal file
View File

@ -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)

15
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucIOState.msg Normal file
View File

@ -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

3
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg Normal file
View File

@ -0,0 +1,3 @@
Header header
int16 payload_num

12
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucReadSingleIO.msg Normal file
View File

@ -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

32
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg Normal file
View File

@ -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

23
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg Normal file
View File

@ -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

3
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Header header
time received_time_stamp

3
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Header header
fanuc_msg_translator_msgs/FanucTrajectoryConverterQueueItem[] goals

7
ros1_noetic_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg Normal file
View File

@ -0,0 +1,7 @@
time received_time_stamp
int16 status_in_queue
int16 status_on_server

63
ros1_noetic_core/fanuc_msg_translator_msgs/package.xml Normal file
View File

@ -0,0 +1,63 @@
<?xml version="1.0"?>
<package format="2">
<name>fanuc_msg_translator_msgs</name>
<version>0.0.0</version>
<description>The fanuc_msg_translator_msgs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="fanuc1@todo.todo">fanuc1</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/fanuc_msg_translator_msgs</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<build_depend>message_generation</build_depend>
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<exec_depend>message_runtime</exec_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

0
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/__init__.py Normal file
View File

92
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_connector.py Normal file
View File

@ -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()

112
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_gui.py Normal file
View File

@ -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()

97
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_status_viewer.py Normal file
View File

@ -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()

64
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/fanuc_control_panel_trajectory_queue.py Normal file
View File

@ -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()

45
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/goal_point_generator.py Normal file
View File

@ -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()

27
ros2_basic_demo_gui/fanuc_control_panel/fanuc_control_panel/my_first_node.py Normal file
View File

@ -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()

26
ros2_basic_demo_gui/fanuc_control_panel/launch/control_panel.launch.py Normal file
View File

@ -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'
)
])

20
ros2_basic_demo_gui/fanuc_control_panel/package.xml Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>fanuc_control_panel</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="christoph.andres@stud.unileoben.ac.at">christoph</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<exec_depend>ros2launch</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
ros2_basic_demo_gui/fanuc_control_panel/resource/fanuc_control_panel Normal file
View File

4
ros2_basic_demo_gui/fanuc_control_panel/setup.cfg Normal file
View File

@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/fanuc_control_panel
[install]
install_scripts=$base/lib/fanuc_control_panel

32
ros2_basic_demo_gui/fanuc_control_panel/setup.py Normal file
View File

@ -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"
],
},
)

25
ros2_basic_demo_gui/fanuc_control_panel/test/test_copyright.py Normal file
View File

@ -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'

25
ros2_basic_demo_gui/fanuc_control_panel/test/test_flake8.py Normal file
View File

@ -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)

23
ros2_basic_demo_gui/fanuc_control_panel/test/test_pep257.py Normal file
View File

@ -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'

0
ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/__init__.py Normal file
View File

307
ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/replay_manager.py Normal file
View File

@ -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()

53
ros2_fanuc_web_connector/fanuc_web_connector/fanuc_web_connector/trajectory_composer.py Normal file
View File

@ -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()

28
ros2_fanuc_web_connector/fanuc_web_connector/launch/web_connector.launch.py Normal file
View File

@ -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'
)
])

20
ros2_fanuc_web_connector/fanuc_web_connector/package.xml Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>fanuc_web_connector</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="christoph.andres@stud.unileoben.ac.at">christoph</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
ros2_fanuc_web_connector/fanuc_web_connector/resource/fanuc_web_connector Normal file
View File

4
ros2_fanuc_web_connector/fanuc_web_connector/setup.cfg Normal file
View File

@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/fanuc_web_connector
[install]
install_scripts=$base/lib/fanuc_web_connector

31
ros2_fanuc_web_connector/fanuc_web_connector/setup.py Normal file
View File

@ -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"
],
},
)

25
ros2_fanuc_web_connector/fanuc_web_connector/test/test_copyright.py Normal file
View File

@ -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'

25
ros2_fanuc_web_connector/fanuc_web_connector/test/test_flake8.py Normal file
View File

@ -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)

23
ros2_fanuc_web_connector/fanuc_web_connector/test/test_pep257.py Normal file
View File

@ -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'

7
ros2_foxy_core/fanuc_crx_description/CMakeLists.txt Normal file
View File

@ -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})

95
ros2_foxy_core/fanuc_crx_description/launch/fanuc_1.launch.py Normal file
View File

@ -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

95
ros2_foxy_core/fanuc_crx_description/launch/fanuc_2.launch.py Normal file
View File

@ -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

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/base.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/j1.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/j2.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/j3.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/j4.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/j5.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/collision/j6.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/base.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/j1.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/j2.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/j3.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/j4.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/j5.stl Normal file
View File

Binary file not shown.

BIN
ros2_foxy_core/fanuc_crx_description/meshes/visual/j6.stl Normal file
View File

Binary file not shown.

18
ros2_foxy_core/fanuc_crx_description/package.xml Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<package format="3">
<name>fanuc_crx_description</name>
<version>2.0.6</version>
<description>TF and robot description for CRX-10iA</description>
<maintainer email="christoph.andres@stud.unileoben.ac.at">Christoph Andres</maintainer>
<license>BSD</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

183
ros2_foxy_core/fanuc_crx_description/robots/crx10ia.xacro Normal file
View File

@ -0,0 +1,183 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from crx10ia.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="crx10ia">
<material name="FANUC_Black">
<color rgba="0 0 0 1.0"/>
</material>
<material name="FANUC_Yellow">
<color rgba="1.00 0.84 0.01 1.00"/>
</material>
<material name="FANUC_Green">
<color rgba="0.43 0.68 0.26 1.00"/>
</material>
<material name="FANUC_White">
<color rgba="0.84 0.84 0.82 1.00"/>
</material>
<!-- links -->
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/base.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_Black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/base.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="J1_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.11729999999999999"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/j1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.11729999999999999"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/j1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="J2_link">
<visual>
<origin rpy="0 0 0" xyz="-0.0 0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/j2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.0 0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/j2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="J3_link">
<visual>
<origin rpy="0 0 0" xyz="-0.0 0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/j3.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.0 0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/j3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="J4_link">
<visual>
<origin rpy="0 0 0" xyz="0.54 0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/j4.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.54 0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/j4.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="J5_link">
<visual>
<origin rpy="0 0 0" xyz="0.0 -0.15 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/j5.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 -0.15 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/j5.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="J6_link">
<visual>
<origin rpy="0 0 0" xyz="0.15999999999999992 0.0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/visual/j6.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="FANUC_Black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.15999999999999992 0.0 0.0"/>
<geometry>
<mesh filename="package://fanuc_crx_description/meshes/collision/j6.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<!-- joints -->
<joint name="J1" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.245"/>
<parent link="base_link"/>
<child link="J1_link"/>
<axis xyz="0 0 1"/>
<limit effort="0" lower="-3.3161255787892263" upper="3.3161255787892263" velocity="2.0943951023931953"/>
</joint>
<joint name="J2" type="revolute">
<origin rpy="0 0 0" xyz="0.0 0 0"/>
<parent link="J1_link"/>
<child link="J2_link"/>
<axis xyz="0 1 0"/>
<limit effort="0" lower="-3.139847324337799" upper="3.139847324337799" velocity="2.0943951023931953"/>
</joint>
<joint name="J3" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.54"/>
<parent link="J2_link"/>
<child link="J3_link"/>
<axis xyz="0 -1 0"/>
<limit effort="0" lower="-1.239183768915974" upper="4.380776422505767" velocity="3.141592653589793"/>
</joint>
<joint name="J4" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<parent link="J3_link"/>
<child link="J4_link"/>
<axis xyz="-1 0 0"/>
<limit effort="0" lower="-3.3161255787892263" upper="3.3161255787892263" velocity="3.141592653589793"/>
</joint>
<joint name="J5" type="revolute">
<origin rpy="0 0 0" xyz="0.54 0 0"/>
<parent link="J4_link"/>
<child link="J5_link"/>
<axis xyz="0 -1 0"/>
<limit effort="0" lower="-3.139847324337799" upper="3.139847324337799" velocity="3.141592653589793"/>
</joint>
<joint name="J6" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.15 0"/>
<parent link="J5_link"/>
<child link="J6_link"/>
<axis xyz="-1 0 0"/>
<limit effort="0" lower="-3.9269908169872414" upper="3.9269908169872414" velocity="3.141592653589793"/>
</joint>
<!-- world coords -->
<link name="wbase"/>
<joint name="base_link-wbase" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.245"/>
<parent link="base_link"/>
<child link="wbase"/>
</joint>
<!-- flange coords -->
<link name="flange"/>
<joint name="J6-flange" type="fixed">
<origin rpy="0 0 0" xyz="0.16 0 0"/>
<parent link="J6_link"/>
<child link="flange"/>
</joint>
</robot>

34
ros2_foxy_core/fanuc_msg_translator_msgs/CMakeLists.txt Normal file
View File

@ -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()

3
ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucPayloadNum.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Header header
int16 payload_num

32
ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatus.msg Normal file
View File

@ -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

23
ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucRobotStatusTriState.msg Normal file
View File

@ -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

3
ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterCancel.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Header header
builtin_interfaces/Time received_time_stamp

3
ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueue.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Header header
fanuc_msg_translator_msgs/FanucTrajectoryConverterQueueItem[] goals

7
ros2_foxy_core/fanuc_msg_translator_msgs/msg/FanucTrajectoryConverterQueueItem.msg Normal file
View File

@ -0,0 +1,7 @@
builtin_interfaces/Time received_time_stamp
int16 status_in_queue
int16 status_on_server

27
ros2_foxy_core/fanuc_msg_translator_msgs/package.xml Normal file
View File

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>fanuc_msg_translator_msgs</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="christoph.andres@stud.unileoben.ac.at">fanuc1</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>std_msgs</depend>
<depend>builtin_interfaces</depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

7
ros2_humble_crx_description/fanuc_crx_description/CMakeLists.txt Normal file
View File

@ -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})

5
ros2_humble_crx_description/fanuc_crx_description/README.md Normal file
View File

@ -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.

93
ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_1.launch.py Normal file
View File

@ -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

93
ros2_humble_crx_description/fanuc_crx_description/launch/fanuc_2.launch.py Normal file
View File

@ -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

BIN
ros2_humble_crx_description/fanuc_crx_description/meshes/collision/base.stl Normal file
View File

Binary file not shown.

BIN
ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j1.stl Normal file
View File

Binary file not shown.

BIN
ros2_humble_crx_description/fanuc_crx_description/meshes/collision/j2.stl Normal file
View File

Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More