ICube-Robotics / ethercat_driver_ros2

Hardware Interface for EtherCAT module integration with ros2_control
https://icube-robotics.github.io/ethercat_driver_ros2/
Apache License 2.0
126 stars 32 forks source link

invalid tag ec_module #10

Closed enriLoniterp closed 1 year ago

enriLoniterp commented 1 year ago

hi @mcbed, im trying your integration in my example code. I've tried to follow your guide in READ.ME but i haven't the expected results.

  1. I add ethercat_driver_ros2 in my worskspace with my other packages to run my RViz simulation with my ethercat driver
  2. Everything compiled, even if put another digital output, it is an Beckhoff el2809 and i made its plugin following the others examples you previously add
  3. i have installed master Igh and it is all ok with it (personally tested)
  4. i had in my ros2_control file the the ethercat-driver as you explained
  5. added EcModule in ros2_control description file
  6. For interfacing controllers with EtherCAT Slave Modules i tried your example with an EtherCAT IO modules etc. etc

in 4. it gives me this error ros2_control

[ros2_control_node-5] terminate called after throwing an instance of 'std::runtime_error'
[ros2_control_node-5]   what():  invalid tag name ec_module

it seems it doesn't understand that it is Hardware resource

and when i don't delete 6. it literally don't start and stop with error on launch.

What is should modify?

mcbed commented 1 year ago

Hi @enriLoniterp , I would need some extra information to help you out.

enriLoniterp commented 1 year ago

Thank you very much for your fast reply.

Consider i'm using MoveIt tutorials packages and i'm using a panda robot. My final target is to turn on a led on my BeckHoff EL2809 16-digital outputs through ROS 2.

To answer your question.

I've taken your ethercat_driver and put in MoveIt_resources directory in the tutorial description with the new plugin.

I've also modified the ros2_controllers.yaml:

# This config file is used by ros2_control
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    panda_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    panda_hand_controller:
      type: position_controllers/GripperActionController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    gpio_command_controller:
       type:gpio_controllers/GpioCommandController

gpio_command_controller:
  ros_parameters:
    gpios:
      - gpio1
    command_interfaces:
        gpio1:
        - di.1

panda_arm_controller:
  ros__parameters:
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7

panda_hand_controller:
  ros__parameters:
    joint: panda_finger_joint1

and in the launch file:

   # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("moveit_resources_panda_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[moveit_config.robot_description, ros2_controllers_path],
        output="screen",
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    panda_arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["panda_arm_controller", "-c", "/controller_manager"]
    )

    ethercat_dig_output = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["ethercat_driver", "-c", "/controller_manager"]
    )

    panda_hand_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["panda_hand_controller", "-c", "/controller_manager"],
    )

    # Warehouse mongodb server
    db_config = LaunchConfiguration("db")
    mongodb_server_node = Node(
        package="warehouse_ros_mongo",
        executable="mongo_wrapper_ros.py",
        parameters=[
            {"warehouse_port": 33829},
            {"warehouse_host": "localhost"},
            {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
        ],
        output="screen",
        condition=IfCondition(db_config),
    )

    return LaunchDescription(
        [
            tutorial_arg,
            db_arg,
            rviz_node,
            rviz_node_tutorial,
            static_tf_node,
            robot_state_publisher,
            move_group_node,
            ros2_control_node,
            joint_state_broadcaster_spawner,
            panda_arm_controller_spawner,
            ethercat_dig_output,
            panda_hand_controller_spawner,
            mongodb_server_node,
        ]
    )
mcbed commented 1 year ago

I don't know if this is the issue but 2 things:

  1. I'm not sure this can work in ros2_control as you have here 2 plugins for the same system which I think will fail.
    
    <hardware>
    <plugin>mock_components/GenericSystem</plugin>
    </hardware>
ethercat_driver/Ethercatdriver 0 100
2. Each ec_module is designed to be a child of a joint, gpio, or sensor
```xml
       <ec_module name="ECModule">
            <plugin>etherca_plugins/ECModule</plugin>
            <param name="alias">0</param>
            <param name="position">1</param>
       </ec_module>

and

 <gpio name="myGPIO">
        <command_interface name="dig_output.1"/></gpio> <-------------------------
        <ec_module name="EL2809">
            <plugin>etherca_plugins/Beckhoff_EL2809</plugin>
            <param name="alias">0</param>
            <param name="position">1</param>
            <param name="do.1">dig_output.2</param>
        </ec_module>
        </gpio> 
enriLoniterp commented 1 year ago

Thank you very much @mcbed,

First thing easier:

 <gpio name="myGPIO">
        <command_interface name="dig_output.1"/></gpio> <-------------------------
        <ec_module name="EL2809">
            <plugin>etherca_plugins/Beckhoff_EL2809</plugin>
            <param name="alias">0</param>
            <param name="position">1</param>
            <param name="do.1">dig_output.2</param>
        </ec_module>
        </gpio> 

I've corrected this problem, sorry.

  1. I'm supposed to put it in inside the tag or should i delete it completely? (below it is how it should be without block of code you mentioned before):

    
    <xacro:macro name="panda_ros2_control" params="name initial_positions_file">
    <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
    
    <ros2_control name="${name}" type="system">
        <hardware>
            <plugin>mock_components/GenericSystem</plugin>
        </hardware>
    
        <hardware>
            <plugin>ethercat_driver/Ethercatdriver</plugin>
            <param name="master_id">0</param>
            <param name="control_frequency">100</param>
        </hardware>
    
        <gpio name="myGPIO">
        <command_interface name="dig_output.1"/></gpio>
        <ec_module name="EL2809">
            <plugin>etherca_plugins/Beckhoff_EL2809</plugin>
            <param name="alias">0</param>
            <param name="position">1</param>
            <param name="do.1">dig_output.1</param>
        </ec_module>
        </gpio> 
    
        <joint name="panda_joint1">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint1']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="panda_joint2">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint2']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="panda_joint3">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint3']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="panda_joint4">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint4']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="panda_joint5">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint5']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="panda_joint6">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint6']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="panda_joint7">
            <command_interface name="position"/>
            <state_interface name="position">
              <param name="initial_value">${initial_positions['panda_joint7']}</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>
    </xacro:macro>
3. If i don't include:
mock_components/GenericSystem>/plugin>

i can't simulated a realistic robot with panda.

And if i'm not disturbing, 
ros-control gave me error on when parsin ros2_control interface:

gpio-command_controller:
   ros_paramenter:
      command_interfaces:
         - dig.output.1    <--------
      state_interfaces:
          - dig_output.1
      gpios:
          - gpio             <--------- not sure about gpios
mcbed commented 1 year ago

Putting the 2 plugins inside a single ros2_control system should not work as it is against the logic of the framework. A ros2_control system is intended to represent a single communication channel. So here you have 2 of them: one for the panda and another for ethercat. You should then have 2 ros2_control systems in your description.

In 2. you still have the closing gpio tag that exludes your ec_module from the component.

For the last, in the gpios parameter you should specify the names of the gpios you want to control. In your case its myGPIO. Then, you specify the interface per listed gpio:

command_interfaces:
    myGPIO:
        - dig.output.1
enriLoniterp commented 1 year ago

Hi @mcbed, sorry for this question, i'm still learning ros2_control.

Thank you for the first suggestion, i should delete mock component and only test with ethercat_drivers, but i still don't know how to make a simple write with etherCAT master with your ethercat_drivers.

For example, i've created a simple application including ros2_control and your etherCAT drivers and i want to actually turn on a led on Beckhoff EL2809, in my experience it's just writing with EC_WRITE8 from my IgH master or other simple macro or functions and the led will turn on. I want to do the same but included in a ROS2 application with your drivers. I've seen you published a simple gpio controller and i'm interested to make it part of my initial simple application. But what are the moves should i do? I think:

  1. add my gpio 16 digital ouput beckhoff on plugin list? Done
  2. add succesfully the ethercat_drivers to my project ros2? Done
  3. write a node in which i instantiate your gpiocontrollerand make a write to my Beckhoff el2809? from now i'm still a bit confused..
  4. ..
  5. ..

Hope you can help, but thanks you a lot anyway!

Enrico

mcbed commented 1 year ago

Did you write the plugin driver for your module inheriting from the ethercat_interface::EcSlave base class ?

In general you should:

  1. write the plugin for your module if not already available in the list. Example for Beckhoff 20xx modules here
  2. set up the description of your ec modules in your hardware components with ec_module tags and map the interfaces of ros2_control to the module channels. Also some modules reuire additional parameters that can be set in the description. This depends on how the plugin is written.
  3. run the gpio_command_controller to interact with the interfaces that are mapped to your module channels. You done need a specific node, you can just run it from command prompt with $ ros2 topic pub ...
enriLoniterp commented 1 year ago
  1. i went on ethercat_driver_ros2/ethercat_plugins/src/beckhoff_plugins and i addedbeckhoff_el2809.cpp which is my 16 digital output gpio taking for example one of your other digital output in the Beckhoff list. (i also have on ek1100 you already prepared)
  2. i put in <ros2_control> my ec_module mapped correctly, not added any additional parameters for now, maybe i will. You can find the ros2_control sample if you did moveit2 tutorials on panda robot, that's it for now (also simpler project, main aspect i am interested is to turn on led on my beckHoff using ROS 2)
  3. i don't understand this point, i run gpio_command_controller using one ros2 controlcommand or it is a node itself which i can run from bash with ros2 topic pub command? by the way i'd rather prefer writing my node because this ethercat write should represent command sent to my beckhoff element which should move my robot theoretically! But i am not sure how to use gpio command controller from code.

I should get joint trajectory controller information, code it in movements to my Beckhoff, and send it with my ethercat master using _gpio_command_controller_

Thanks very much again, Enrico

enriLoniterp commented 1 year ago

Hi @mcbed,

i'm also testing ethercat with your scara_robot project! I replied the procedure and it gave me same error:


controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    scara_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    scara_position_controller:
      type: position_controllers/JointGroupPositionController

    scara_joint_velocity_controller:
      type: scara_joint_velocity_controller/ScaraJointVelocityController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    gpio_command_controller:
      type: gpio_controllers/GpioCommandController

scara_trajectory_controller:
  ros__parameters:
    command_interfaces:
      - position
    state_interfaces:
      - position
    joints:
      - joint1
      - joint2
      - joint3

scara_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3

scara_joint_velocity_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3

gpio_command_controller:
  ros_parameters:
    command_interfaces:
      myGPIO:
        - dig_output.1  <--- here

At "here" says to me:

[ros2_control_node-2] [ERROR] [1663583337.726435874] [rcl]: Failed to parse global arguments
[ros2_control_node-2] terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
[ros2_control_node-2]   what():  failed to initialize rcl: Couldn't parse params file: '--params-file /home/rosdev/scara_ws/install/scara_description/share/scara_description/config/scara_controllers.yaml'. Error: Sequences can only be values and not keys in params. Error at line 53

Still not understand what it it the problem :(

scara.ros2_control.urdf

<?xml version="1.0"?>
<robot name = "scara" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <ros2_control name="scara" type="system">

        <hardware>
            <plugin>mock_components/GenericSystem</plugin>
            <plugin>scara_hardware/ScaraRobot</plugin>
            <plugin>ethercat_driver/EthercatDriver</plugin>
            <param name="master_id">0</param>
            <param name="control_frequency">100</param> 

            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>

            <gpio name="myGPIO">
                <command_interface name="dig_output.1"/>
                        <!-- <command_interface name="dig_output.2"/>--> -->
                    <ec_module name="EL2809">
                        <plugin>ethercat_plugins/Beckhoff_EL2809</plugin>
                            <param name="alias">0</param>
                            <param name="position">1</param>
                            <param name="do.1">dig_output.1</param>
                    </ec_module>
             </gpio>

        <joint name="joint1">
            <command_interface name="position" />
            <state_interface name="position">
                <param name="initial_value">0.0</param>
                <param name="min">-1.57</param>
                <param name="max">1.57</param>
            </state_interface>
            <state_interface name="velocity"> 
                <param name="initial_value">0.0</param> 
            </state_interface>
        </joint>
        <joint name="joint2">
            <command_interface name="position"/>
            <state_interface name="position">
                <param name="initial_value">0.5.</param>
                <param name="min">-1.57</param>
                <param name="max">1.57</param>
            </state_interface>
            <state_interface name="velocity"> 
                <param name="initial_value">0.0</param> 
            </state_interface>
        </joint>
        <joint name="joint3">
            <command_interface name="position"/>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
                <param name="min">0</param>
                <param name="max">0.3</param>
            </state_interface>
            <state_interface name="velocity"> 
                <param name="initial_value">0.0</param> 
            </state_interface>
        </joint>
    </ros2_control>

</robot>

scara.launch.py

# Copyright 2022 ICube Laboratory, University of Strasbourg
#
# 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 launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    # Get URDF via xacro
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name='xacro')]),
            ' ',
            PathJoinSubstitution(
                [FindPackageShare('scara_description'), 'config', 'scara.config.xacro']
            ),
        ]
    )
    robot_description = {'robot_description': robot_description_content}

    robot_controllers = PathJoinSubstitution(
        [
            FindPackageShare('scara_description'),
            'config',
            'scara_controllers.yaml',
        ]
    )
    rviz_config_file = PathJoinSubstitution(
        [FindPackageShare('scara_description'), 'rviz', 'scara.rviz']
    )

    control_node = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[robot_description, robot_controllers],
        output='both',
    )
    robot_state_pub_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='both',
        parameters=[robot_description],
    )
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='log',
        arguments=['-d', rviz_config_file],
    )

    joint_state_broadcaster_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster'],
    )

    robot_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['scara_position_controller'],
    )
    ethercat_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['gpio_command_controller'],
    )

    nodes = [
        rviz_node,
        control_node,
        robot_state_pub_node,
        joint_state_broadcaster_spawner,
        robot_controller_spawner,
    ]

    return LaunchDescription(nodes)
mcbed commented 1 year ago

first, you cannot use multiple plugins for a ros2_control system component :

<ros2_control name="scara" type="system">
        <hardware>
            <plugin>mock_components/GenericSystem</plugin>
            <plugin>scara_hardware/ScaraRobot</plugin>
            <plugin>ethercat_driver/EthercatDriver</plugin>
            <param name="master_id">0</param>
            <param name="control_frequency">100</param> 

            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
</ros2_control>

if you want both, you need to create 2 systems:

<ros2_control name="ecdriver" type="system">
        <hardware>
            <plugin>ethercat_driver/EthercatDriver</plugin>
            <param name="master_id">0</param>
            <param name="control_frequency">100</param> 
        </hardware>
        <!-- ec_modules-->
</ros2_control>
<ros2_control name="scara" type="system">
        <hardware>
            <plugin>mock_components/GenericSystem</plugin>
        </hardware>
        <!-- joints -->
</ros2_control>
mcbed commented 1 year ago

Second, in your controller cofiguration you are missing the gpios see doc :

gpio_command_controller:
  ros_parameters:
    gpios:
        - myGPIO
    command_interfaces:
        myGPIO:
            - dig_output.1
enriLoniterp commented 1 year ago

Should work this #way?

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    scara_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    scara_position_controller:
      type: position_controllers/JointGroupPositionController

    scara_joint_velocity_controller:
      type: scara_joint_velocity_controller/ScaraJointVelocityController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    gpio_command_controller:
      type: gpio_controllers/GpioCommandController

scara_trajectory_controller:
  ros__parameters:
    command_interfaces:
      - position
    state_interfaces:
      - position
    joints:
      - joint1
      - joint2
      - joint3

  scara_position_controller:
    ros__parameters:
      joints:
        - joint1
        - joint2
        - joint3

  scara_joint_velocity_controller:
    ros__parameters:
      joints:
        - joint1
        - joint2
        - joint3

  gpio_command_controller:
    ros__parameters:
        gpios:
          - myGPIO
        command_interfaces:
          myGPIO:
            - dig_output.1
        state_interfaces:
            myGPIO:
            - dig_output.1

Second, in your controller cofiguration you are missing the gpios see doc :

gpio_command_controller:
  ros_parameters:
    gpios:
        - myGPIO
    command_interfaces:
        myGPIO:
            - dig_output.1
mcbed commented 1 year ago

Looks good to me. Just the state_interfaces tag is useless as not used by the controller

enriLoniterp commented 1 year ago

Hi @mcbed,

i agree but i remember it seems i had a problem making everything working without it, anyway i will fix it, for now i'm just happy is connecting!

In particular i would like to ask you some new information about ethercat drivers, It seems to connect well, and i think i'm sending the right value to my EL_2809usin ros2 topic pub but i'm not sure i understand every passage. three components are involved:

Thank you very much for the great help! Your helping me a lot!

Enrico

enriLoniterp commented 1 year ago

Hi @mcbed ,

Another thing i would ask you is how can i send repeated messagges to my BeckHoff2809 in order to maintain powered on the led. My solution was quite simple in Igh EthercatMaster:


void cyclic_task()
{

    ecrt_master_receive(master);
    ecrt_domain_process(domain1);

    // check process data state
    check_domain1_state();
    if (counter) {
        counter--;
    } else { // do this at 1 Hz
        counter = FREQUENCY;

        // calculate new process data
        blink = !blink;
        // check for master state (optional)
        check_master_state();
        // check for slave configuration state(s) (optional)
        // check_slave_config_states();
    }

#if 0
    // read process data
    printf("AnaIn: state %u value %u\n",
            EC_READ_U8(domain1_pd + off_ana_in_status),
            EC_READ_U16(domain1_pd + off_ana_in_value));
#endif

#if 1
    u_int8_t result;
    // write process data
    EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x00 : 0x01);
    EC_WRITE_U8(domain1_pd + off_dig_out2, blink ? 0x00 : 0x08);
    result = EC_READ_U8(domain1_pd + off_dig_in);
    printf("input slave3 uint: %x\n", result);
    printf("input slave3 bit1: %c\n\n", result & 0x01);
#endif
    // send process data
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
}

As you can see i wrote periodically :

  EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x00 : 0x01);
  EC_WRITE_U8(domain1_pd + off_dig_out2, blink ? 0x00 : 0x08);

the same i want to do with you plugin but it seems that proceed_data in the plugin of my actuator gets error if include any cycles. Maybe i should put this cylce inside the gpio_controller?

mcbed commented 1 year ago

Hi, It depends on how you did your plugin I suppose. In the case of our plugins, the value of the state_interface remains the same until a controller changes it so if you want your led to blink you need to publish 1/0/1/... in your topic. You can do it either in a dedicated controller or using a ros2 node.

enriLoniterp commented 1 year ago

Hi, i'm using ethercat_driverplugin and the controller involved is gpio_controller, maybe i should change update method in the controller.

So, i don't change the controller, but i need a simple publisher like this:

 public:
    MinimalPublisher()
    : Node("minimal_publisher"), count_(0)
    {
      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);   
//using CmdType std_msgs::msg::Float64MultiArray;
      timer_ = this->create_wall_timer(                             
      500ms, std::bind(&MinimalPublisher::timer_callback, this));                           
    }

As you can see i stated "using Float64MultiArray " because it was included in your controller but actually i'm not interested, for my purpose it is only write a simple int value. Should i change it and publish on simple int message?

Thank you again, Enrico

mcbed commented 1 year ago

The message type for the topic is necessary a Float64MultiArray. If you want to publish a single int value just send an array of 1 float element and read it as an int.

enriLoniterp commented 1 year ago

hi @mcbed , i did it, my led is ON!!! Thank you very much for your help, your work has been essenial for me!!

Now i have to understand better what i'm doing.

  1. Can i change Float64MultiArray in cmdType with something more suitable for my options?
  2. Prev i've powered 1 led, now i need to achieve anything with my 16 digital output, so i ask:
  3. should i put a digital output.x for each of my digital output ? and how can i wrote 16 bit with just an array of float? (this brings me back to first question) Thank you again very much!!
mcbed commented 1 year ago

Congrats ! I am happy you could make it work !

  1. You can but this requires to develop your custom controller.
  2. In my applications I write a message array of 16 floats and use the gpio_controller to pass them as state_interfaces to the ec module plugin that read them according to the mapping you pass as module parameters.
enriLoniterp commented 1 year ago

Thank you!

I think i will built my custom controller, this code should work on a real robot one day! It should be something representing a real motor, i think maybe they should be more plugins which will receive one value!!

I don't understand something of you 2.. Why you said state_interfaces, you are refering to command_interfaces? I've always thought that state_interfaces is for reading from the hardware_interfaces while command to write.

mcbed commented 1 year ago

I don't understand something of you 2.. Why you said state_interfaces, you are refering to command_interfaces? I've always thought that state_interfaces is for reading from the hardware_interfaces while command to write.

Sorry, too fast typing, you are right of course!

enriLoniterp commented 1 year ago

hi @mcbed, two questions:

  1. My digital output has 2 blocks of 8bit output value: };
    
    ...... // code
    ec_sync_info_t syncs_[3] = {
        {0, EC_DIR_OUTPUT, 8, pdos_ + 0, EC_WD_ENABLE},
        {1, EC_DIR_OUTPUT, 8, pdos_ + 8, EC_WD_ENABLE},
        {0xff}
    };
    DomainMap domains_ = {
        {0, {0} },     <-- first block
        {1, {8} }      <-- second block
    };
    };

my addresses start at 0x7000 (first 8 bit) 0x7080 (second 8 bit)
i can't write properly on the second. How am i supposed to make 2 EC_WRITEU8 on this output?
consider i have assigned digital output in ros2 controller file!

2. I've noticed that if i sends every minutes and set `ros2_control` frequency at 2Hz , it still at 2Hz freq.. This mean ros2_control publish everytime and not just only one time. IIs it possible to write on gpio el2809 only one time?
3. And it is possible to have a feedback mechanism from the Bechoff gpio?

Thank you again @mcbed 
mcbed commented 1 year ago

hi,

1. My digital output has 2 blocks of 8bit output value:  };
...... // code
    ec_sync_info_t syncs_[3] = {
        {0, EC_DIR_OUTPUT, 8, pdos_ + 0, EC_WD_ENABLE},
        {1, EC_DIR_OUTPUT, 8, pdos_ + 8, EC_WD_ENABLE},
        {0xff}
    };
    DomainMap domains_ = {
        {0, {0} },     <-- first block
        {1, {8} }      <-- second block
    };
};

my addresses start at 0x7000 (first 8 bit) 0x7080 (second 8 bit) i can't write properly on the second. How am i supposed to make 2 EC_WRITEU8 on this output? consider i have assigned digital output in ros2 controller file!

what are your channels and pdos ? I think the problem comes from the way you configure the domains.

  1. I've noticed that if i sends every minutes and set ros2_control frequency at 2Hz , it still at 2Hz freq.. This mean ros2_control publish everytime and not just only one time. IIs it possible to write on gpio el2809 only one time? I do not understand the problem here, sorry. Isn't the freq what you set ?
  2. And it is possible to have a feedback mechanism from the Bechoff gpio? This depends on the module I suppose. You would need to check if the register value fits what you want. Alternatively, in the plugin you can define a state_interface that takes the value of the command_interface just after writing to the module. This is something we do here
enriLoniterp commented 1 year ago
  1. first problem solved!, just written 16 and set one domain, now it's working!

  2. Yes i put that frequency but if i send one value each minute from the topic, the ros2_control write to the hardware interface the same message. my idea is to send one value and then stop to write on hardware.

  3. Yes, my intent is have an ack and print it at least! (advanced idea would be comunicate with MoveIt that value)

As always, thank you very much!