Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Create a simple EtherCAT IO network (Implementation Notes)
Description:Keywords: EtherCAT
Tutorial Level: ADVANCED
Deprecation notice
This tutorial is kept for archival purposes only. Packages mentioned may not be available any more and the procedure outlined below may stop working at any time.
Hardware Setup (example)
- T61 Thinkpad Laptop (built in Ethernet card)
IO rack: EtherCAT Couper, 8 DI, & 8DO (all Beckhoff)
Beckhoff EtherCAT Drivers
Install OROCOS toolchain for ROS
- Install Simple Open !EtherCAT Master (soem) library from source:
git clone https://github.com/orocos/rtt_soem.git
Make soem by running catkin_make.
- Connect EtherCAT device to ethernet port. EtherCAT works over standard ethernet cable. Make a direct connection, I do not think connections through switches work since EtherCAT couplers do not have IP addresses (typcially)
In the soem_core package give the slaveinfo binary root access to socket commands:
sudo setcap cap_net_raw+ep bin/slaveinfo
(you might have to install this utility: sudo apt-get install libcap2-bin)
Execute the slaveinfo utility:
rosrun soem_core slaveinfo <device, typcially eth0>
When the utility successfully executes a list of connected slaves is printed (this list is generated by auto discovery).(This step may not be required on the latest soem version) The soem stack has a bug that fails to recognize IO sizes for unknown modules. The following patch fixes the bug (BUG has been submitted). Patch the soem stack with the following:
diff --git a/soem_master/soem_master_component.cpp b/soem_master/soem_master_component.cpp index e166ad0..dc6b460 100644 --- a/soem_master/soem_master_component.cpp +++ b/soem_master/soem_master_component.cpp @@ -79,6 +79,8 @@ bool SoemMasterComponent::configureHook() // wait for all slaves to reach PRE_OP state ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); + ec_config(FALSE, &m_IOmap); + for (int i = 1; i <= ec_slavecount; i++) { SoemDriver @@ -89,6 +91,8 @@ bool SoemMasterComponent::configureHook() m_drivers.push_back(driver); log(Info) << "Created driver for " << ec_slave[i].name << ", with address " << ec_slave[i].configadr + << ", output bits " << ec_slave[i].Obits + << ", input bits " << ec_slave[i].Ibits << endlog(); //Adding driver's services to master component this->provides()->addService(driver->provides());
Run rosmake from the soem_master directory to compile in the patch
Edit the test.ops script to start the master up and map OROCOS messages to ROS topics
import("soem_beckhoff_drivers") loadComponent("Master","soem_master::SoemMasterComponent") Master.displayAvailableDrivers() # Default nic is eth0, set if different. e.g. #Master.ifname = "eth1" # Crawls the network and identifies attached devices. Devices are loaded # (if a driver exists) and given names "Slave_1***". The *** appear to be # the module order (at least this is the case for a single rack) Master.configure() # Setting the update period (in seconds). This determines the rate at which # the data is published on the ROS side (setting this value to zero turns # off updating Master.setPeriod(0.05) # This stars the Master "task" running. It will update at the period set # above Master.start() # This command remaps OROCOS topics to ROS topics stream("Master.Slave_1002.bits", ros.topic("DI")) stream("Master.Slave_1003.bits", ros.topic("DO"))
In the ocl package give the deployer-gnulinux binary (called by launch file below) root access to socket commands:
sudo setcap cap_net_raw+ep bin/deployer-gnulinux
- From the command line, launch the master:
roslaunch soem_master soem_master_test.launch
The launch file launches orocos and loads the test.ops script.
Other Notes:
Enabling OROCOS Info Logging: export ORO_LOGLEVEL=5 (see The Orocos Component Builder's Manual - Logging)
Orocos messages to/from ROS messages (see rtt_ros_integration_example). Example script call: stream("Master.Slave_1002.bits", ros.topic("DI_bits"))