search

ros-industrial / ros2_canopen

CANopen driver framework for ROS2
https://ros-industrial.github.io/ros2_canopen/manual/rolling/
141 stars 60 forks source link

Functioning with custom device driver. #46

Closed 8vin8 closed 1 year ago

8vin8 commented 2 years ago

Hello Team,

I am a newbie to CANopen and ROS2, I have gone through this https://ros-industrial.github.io/ros2_canopen/ as well, but not able to understand how can I add a device driver and not able to understand how this new repository is functioning. Do you have some more documentation where I can understanding how its working. I have also gone through this documentation and implementation of master slave part https://opensource.lely.com/canopen/docs/installation/ I need more information how to add a device driver into this CANopen stack.

I would be grateful for your help. Regards

hellantos commented 2 years ago

Hi,

Currently, there is no conprehensive documentation on driver development. We will add it once the interfaces are stable.

If you want to create a driver now, it can either inherit from ros2_canopen::BaseDriver or from ros2_canopen::ProxyDriver. Both have an LelyBridge member called driver. The LelyBridge driver member provides functions to for reading and writing sdos as well as transmitting tpdo and nmt commands. (Check in lely_bridge.hpp file, there is some documentation)

You will also need to override on_nmt which is a callback tracking nmt_state of the slave and on_rpdo which is a callback for handling pdo messages sent by the slave. (Check in canopen_base_driver.hpp there is some documentation and check in canopen_proxy_driver.cpp for how to use it)

8vin8 commented 2 years ago

Hallo Christoph,

Thank you very much for your reply. I will try to implement it.

tback123 commented 1 year ago

Hi there,

Appologies I am also a bit new when it comes to CanOpen and the ros2_canopen library. I'm trying to create my own device driver using the ProxyDriver (or BaseDrive, i don't really mind), and I understand how to implenent the class but am not sure where to actually init the driver.

The doc string in canopen_base_driver.cpp states the init function does the following

Initializes the driver, adds it to the CANopen Master.
This function needs to be executed inside the masters
event loop or the masters thread!

My question: Where is the 'masters event loop'? I currently just have a configuration package with some launch files? I feel like I might be missing something fundamental here.

Appologies for the question, and thankyou in advance for your help!

Regards

tback123 commented 1 year ago

Hey @ipa-cmh - Sorry to tag but wasn't sure if you would see the above question with the issue closed. Would really appreciate any direction.

Regards, Tom

hellantos commented 1 year ago

Hi Tom,

We are currently reworking the strucutre for custom device drivers. What kind of device do you want to write a driver for?

Best

tback123 commented 1 year ago

Hey Christoph,

Thankyou for getting back to me :)

I'm writing a driver for a custom device (are using an implementation of the CanOpen stack on a in-house designed stm32 based board). The device only needs to be able to send and recieve PDO's at a very basic level, so don't need much 'fancy' functionality.

We've written a driver based on the proxy driver, and it looks good, but am just not sure where to add the deivce to the configuration or how to launch it. Using ROS2 Galactic for reference.

Thank you again, Tom

hellantos commented 1 year ago

Hi Tom,

Sorry for the late answer. You would need to make it a component. Once that is done, you should be able to include it into the bus.yaml file.

TO make it a component you need to add: 

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE([namespace]::[DriverClass])

To the bottom of your drivers .cpp file.

In your CMakeLists.txt you need to make your driver a shared lib and add something like this:

rclcpp_components_register_nodes(lifecycle_base_driver "ros2_canopen::LifecycleBaseDriver")
set(node_plugins "${node_plugins}ros2_canopen::LifecycleBaseDriver;$<TARGET_FILE:lifecycle_base_driver >\n")

Also make sure you install the libraries you create.

8vin8 commented 1 year ago

Hello Team and @ipa-cmh ,

For mock slave i am using simple.eds and slave.cpp file. (implementation of simple incremental counter)

When I launch the master and mock slave, i am able to get the data on the CAN bus but i am not able to get the data via ros topics: /proxy_device/tpdo and /proxy_device/rpdo.

Maybe I am missing something and I would be thankful for your help. I have attached the logs below.

ros2 topic list -t /device_container_node/transition_event [lifecycle_msgs/msg/TransitionEvent] /parameter_events [rcl_interfaces/msg/ParameterEvent] /proxy_device/nmt_state [std_msgs/msg/String] /proxy_device/rpdo [canopen_interfaces/msg/COData] /proxy_device/tpdo [canopen_interfaces/msg/COData] /proxy_device/transition_event [lifecycle_msgs/msg/TransitionEvent] /rosout [rcl_interfaces/msg/Log]

ros2 topic echo /proxy_device/tpdo NOTHING ON TERMINAL

ros2 topic echo /proxy_device/rpdo NOTHING ON TERMINAL

when Launch file is launched

candump output on terminal:

candump can0 can0 701 [1] 00 can0 000 [2] 82 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 602 [8] 40 00 10 00 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 602 [8] 80 00 10 00 00 00 04 05 can0 602 [8] 40 00 10 00 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 602 [8] 80 00 10 00 00 00 04 05 can0 602 [8] 40 00 10 00 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 702 [1] 00 can0 202 [4] 00 00 00 00 can0 182 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 602 [8] 80 00 10 00 00 00 04 05 can0 602 [8] 40 00 10 00 00 00 00 00 can0 582 [8] 43 00 10 00 00 00 00 00 can0 602 [8] 40 18 10 01 00 00 00 00 can0 582 [8] 43 18 10 01 60 03 00 00 can0 602 [8] 2B 17 10 00 64 00 00 00 can0 582 [8] 60 17 10 00 00 00 00 00 can0 602 [8] 23 00 14 01 02 02 00 80 can0 582 [8] 60 00 14 01 00 00 00 00 can0 602 [8] 2F 00 14 02 01 00 00 00 can0 582 [8] 60 00 14 02 00 00 00 00 can0 602 [8] 2F 00 16 00 00 00 00 00 can0 582 [8] 60 00 16 00 00 00 00 00 can0 602 [8] 23 00 16 01 20 00 00 40 can0 582 [8] 60 00 16 01 00 00 00 00 can0 602 [8] 2F 00 16 00 01 00 00 00 can0 582 [8] 60 00 16 00 00 00 00 00 can0 602 [8] 23 00 14 01 02 02 00 00 can0 080 [0] can0 582 [8] 60 00 14 01 00 00 00 00 can0 202 [4] 00 00 00 00 can0 602 [8] 23 00 18 01 82 01 00 80 can0 582 [8] 60 00 18 01 00 00 00 00 can0 602 [8] 2F 00 18 02 01 00 00 00 can0 582 [8] 60 00 18 02 00 00 00 00 can0 602 [8] 2F 00 1A 00 00 00 00 00 can0 582 [8] 60 00 1A 00 00 00 00 00 can0 602 [8] 23 00 1A 01 20 00 01 40 can0 582 [8] 60 00 1A 01 00 00 00 00 can0 602 [8] 2F 00 1A 00 01 00 00 00 can0 582 [8] 60 00 1A 00 00 00 00 00 can0 602 [8] 23 00 18 01 82 01 00 00 can0 582 [8] 60 00 18 01 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 182 [4] 00 00 00 00 can0 702 [1] 05 can0 080 [0] can0 202 [4] 01 00 00 00 can0 182 [4] 02 00 00 00 can0 080 [0] can0 202 [4] 01 00 00 00 can0 182 [4] 02 00 00 00 can0 080 [0] can0 202 [4] 02 00 00 00 can0 182 [4] 02 00 00 00 can0 080 [0] can0 202 [4] 02 00 00 00 can0 182 [4] 02 00 00 00 can0 080 [0]

hellantos commented 1 year ago

Regarding the tpdo topic, this is only subscribed to. If you publish messages to it, it will send them via tpdo.

The rpdo topic is the one that should show something. The call back for publishing can be found in canopen_proxy_driver/include/node_interfaces/node_canopen_proxy_driver_impl.hpp

template <class NODETYPE>
void NodeCanopenProxyDriver<NODETYPE>::on_rpdo(ros2_canopen::COData d)
{
    if (this->activated_.load())
    {
        RCLCPP_INFO(
            this->node_->get_logger(),
            "Slave %hhu: Sent PDO index %hu, subindex %hhu, data %x",
            this->lely_driver_->get_id(),
            d.index_,
            d.subindex_,
            d.data_);
        auto message = canopen_interfaces::msg::COData();
        message.index = d.index_;
        message.subindex = d.subindex_;
        message.data = d.data_;
        message.type = static_cast<uint8_t>(d.type_);
        rpdo_publisher->publish(message);
    }
}

This will fail silently when the driver is not marked as activated. This might be the issue. I will create a launch test in canopen_tests for this. Currently we only cover sdos with tests.

8vin8 commented 1 year ago

Hallo @ipa-cmh ,

Thank you very much for taking the time to read my concern and replying. I will figure it out. Regards, Vin

8vin8 commented 1 year ago

Hallo Team and @ipa-cmh,

I am using the Galactic branch, and was using OnRpdoWrite function which might caused the above problem of "no data on ros topic /proxy_device/rpdo. When I removed the function, its working fine. Thank you and sorry for the miscommunication.

Logs attached below.

[device_container_node-2] [INFO] [1667566652.409385929] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 1 [device_container_node-2] [INFO] [1667566653.289790524] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 2 [device_container_node-2] [INFO] [1667566654.169310239] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 3 [device_container_node-2] [INFO] [1667566655.069411589] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 4 [device_container_node-2] [INFO] [1667566655.956112383] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 5 [device_container_node-2] [INFO] [1667566656.849876955] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 6 [device_container_node-2] [INFO] [1667566657.729956659] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 7 [device_container_node-2] [INFO] [1667566658.629411400] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 8 [device_container_node-2] [INFO] [1667566659.509752275] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data 9 [device_container_node-2] [INFO] [1667566660.410078682] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data a [device_container_node-2] [INFO] [1667566661.295442698] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data b [device_container_node-2] [INFO] [1667566662.170084811] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data c [device_container_node-2] [INFO] [1667566663.070003330] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data d [device_container_node-2] [INFO] [1667566663.949613915] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data e [device_container_node-2] [INFO] [1667566664.829365550] [proxy_device]: Slave 2: Sent PDO index 16385, subindex 0, data f

can0 701 [1] 05 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00 can0 080 [0] can0 202 [4] 00 00 00 00

ros2 topic echo /proxy_device/rpdo index: 16385 subindex: 0 data: 12 type: 0

index: 16385 subindex: 0 data: 13 type: 0

index: 16385 subindex: 0 data: 14 type: 0

index: 16385 subindex: 0 data: 15 type: 0

hellantos commented 1 year ago

@8vin8 great thanks for the heads up!