ros2 migration - Githubissues

fllay commented 1 year ago

I edited witmotion_ros.cpp, witmotion_ros_node.cpp and witmotion.h to add ros2 publisher and service. I still struggle with witmotion_ros_node.cpp.

twdragon commented 1 year ago

@fllay you removed compilation of the underlying library and linking it to the main node from CMakeLists.txt. This is why the node does not link properly. Please check your build pipeline.

fllay commented 1 year ago

I fixed link problem. Now, only how to use ros::AsyncSpinner spinner(2); in ros2 since there is no AsyncSpinner in ros2.

twdragon commented 1 year ago

@fllay please refer here to solve: https://discourse.ros.org/t/async-executor-in-ros2/1575

It seems working.

twdragon commented 1 year ago

@fllay if you would be successful to test it, I will create the ROS2 branch here, but now I can not unless the code is not feasible to be run.

fllay commented 1 year ago

I have tried https://discourse.ros.org/t/async-executor-in-ros2/1575. it seems to be ok but now I have another problem. Somehow, It cannot communicate with serial port.

pi@pi-desktop:~/ros2_ws$ ros2 run witmotion witmotion_ros_node --ros-args --params-file /home/pi/ros2_ws/src/witmotion_IMU_ros/config/wt61c.yml
[INFO] [1667615027.287959826] [ROSWitmotionSensorController]: Controller started@@@@@@
Opening device "ttyAMA1" at 115200 baud
Instantiating timer at 30 ms
^C[INFO] [1667615031.384384523] [rclcpp]: signal_handler(signum=2)
[ERROR] [1667615031.387922914] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667615031.388039951] [ROSWitmotionSensorController]: Entering SUSPENDED state
QObject::killTimer: Timers cannot be stopped from another thread
QObject::~QObject: Timers cannot be stopped from another thread
QSocketNotifier: Socket notifiers cannot be enabled or disabled from another thread
Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667615031.390323922] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667615031.390413366] [ROSWitmotionSensorController]: Entering SUSPENDED state
Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667615031.390650217] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667615031.390727957] [ROSWitmotionSensorController]: Entering SUSPENDED state
Suspending TTL connection, please emit RunPoll() again to proceed!

The error message comes from witmotion_IMU_QT. is any way to verify only witmotion_IMU_QT?

Hardware is same as the one I use with ROS1. I also fixed parameter yaml file so that it can be now used in ROS2. All parameters look ok for me. But the serial port read is fail.

twdragon commented 1 year ago

@fllay please try to compile the library separately and run one of the controller applications (I suggest witmotionctl-jy901) with the same baudrate on the same port to see what will happen.

The procedure is quite easy:

git clone https://github.com/ElettraSciComp/witmotion_IMU_QT.git witmotion-uart-qt 
cd witmotion-uart-qt && mkdir build && cd build
cmake ..
make
./witmotionctl-jy901

twdragon commented 1 year ago

@fllay if the controller application will print the result, it should be checked that the start() function is called for the sensor controller object to start the Qt event loop with decoder functions, but it seems working already.

fllay commented 1 year ago

I think I know where the problem is but I am still looking for a solution. The problem is

    rclcpp::executors::MultiThreadedExecutor executor;
    auto executor_spin_lambda = [&executor]() {
      executor.spin();
    };

    executor.add_node(node);
    {
        std::thread spin_thread(executor_spin_lambda);
        spin_thread.join();
    }
   int result = app.exec();
   return result;

After spin_thread.join(); this int result = app.exec(); code will not run until I hit ctrl-c then I got

Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667615031.390323922] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667615031.390413366] [ROSWitmotionSensorController]: Entering SUSPENDED state
Suspending TTL connection, please emit RunPoll() again to proceed!

then it stops. This output is from int result = app.exec();. I also try to put spin_thread.join(); after int result = app.exec(); but it does not work either.

The solution is to execute QCoreApplication app(argc, argv); in std::thread but I don't know how. Any advice?

That is the only problem. I have tested ros2 publisher using timer. It works just fine.

twdragon commented 1 year ago

I think I know where the problem is but I am still looking for a solution. The problem is
    rclcpp::executors::MultiThreadedExecutor executor;
    auto executor_spin_lambda = [&executor]() {
      executor.spin();
    };

    executor.add_node(node);
    {
        std::thread spin_thread(executor_spin_lambda);
        spin_thread.join();
    }
   int result = app.exec();
   return result;
After spin_thread.join(); this int result = app.exec(); code will not run until I hit ctrl-c then I got
Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667615031.390323922] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667615031.390413366] [ROSWitmotionSensorController]: Entering SUSPENDED state
Suspending TTL connection, please emit RunPoll() again to proceed!
then it stops. This output is from int result = app.exec();. I also try to put spin_thread.join(); after int result = app.exec(); but it does not work either.

The solution is to execute QCoreApplication app(argc, argv); in std::thread but I don't know how. Any advice?

That is the only problem. I have tested ros2 publisher using timer. It works just fine.

@fllay here join() is the blocking call. I found the following solution here:

// Create a Node and an Executor.
rclcpp::executors::SingleThreadedExecutor executor1;
auto node1 = rclcpp::Node::make_shared("node1");
executor1.add_node(node1);

// Create another.
rclcpp::executors::SingleThreadedExecutor executor2;
auto node2 = rclcpp::Node::make_shared("node2");
executor2.add_node(node2);

// Spin the Executor in a separate thread.
std::thread spinThread([&executor2]() {
    executor2.spin();
});

So, please, check is join() call could be handled in a different way or to be excluded in favour of another std::thread API call.

fllay commented 1 year ago

Let take a look at main() and its output

int main(int argc, char *argv[]) {
  struct sigaction sigIntHandler;
  sigIntHandler.sa_handler = handle_shutdown;
  sigemptyset(&sigIntHandler.sa_mask);
  sigIntHandler.sa_flags = 0;
  sigaction(SIGINT, &sigIntHandler, NULL);

  QCoreApplication app(argc, argv);

  rclcpp::init(argc, argv);

  ROSWitmotionSensorController &controller =
      ROSWitmotionSensorController::Instance();
  auto node = controller.Start();

  rclcpp::executors::SingleThreadedExecutor executor;

  executor.add_node(node);

  std::thread spinThread([&executor, &app]() {
    executor.spin();

  });

  spinThread.detach();
  RCLCPP_INFO(rclcpp::get_logger("MinimalPublisher"), "QT spin !!!!!");
  app.exec();

  return 0;
}

The output is

pi@pi-desktop:~/ros2_ws$ ros2 run witmotion witmotion_ros_node --ros-args --params-file /home/pi/ros2_ws/src/witmotion_IMU_ros/config/wt61c.yml
[INFO] [1667878430.205058767] [ROSWitmotionSensorController]: Controller started
Opening device "ttyAMA1" at 115200 baud
[INFO] [1667878430.205514562] [ROSWitmotionSensorController]: Initiating RTC pre-synchonization: current timestamp 2022-11-08T10:33:50.205
[INFO] [1667878430.205577692] [ROSWitmotionSensorController]: Configuration ROM: lock removal started
Instantiating timer at 30 ms
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x69
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x33
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x32
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x31
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x30
Configuration packet sent, flushing buffers...
Configuration completed
[INFO] [1667878435.207298822] [ROSWitmotionSensorController]: RTC pre-synchonization completed, saving configuration
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x0
Configuration packet sent, flushing buffers...
Configuration completed
[INFO] [1667878436.207643297] [ROSWitmotionSensorController]: RTC synchronized
[INFO] [1667878436.208435110] [MinimalPublisher]: QT spin !!!!!
[ERROR] [1667878436.208680980] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667878436.208801294] [ROSWitmotionSensorController]: Entering SUSPENDED state
[INFO] [1667878436.208926868] [witmotion]: Publishing: 'Hello, world! '
QObject::killTimer: Timers cannot be stopped from another thread
QObject::~QObject: Timers cannot be stopped from another thread
QSocketNotifier: Socket notifiers cannot be enabled or disabled from another thread
Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667878436.211071364] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667878436.211211790] [ROSWitmotionSensorController]: Entering SUSPENDED state
Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667878436.211386234] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!

:
:

[ERROR] [1667878436.233951006] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667878436.233982080] [ROSWitmotionSensorController]: Entering SUSPENDED state
Suspending TTL connection, please emit RunPoll() again to proceed!
[ros2run]: Segmentation fault

I think this part

Opening device "ttyAMA1" at 115200 baud
[INFO] [1667878430.205514562] [ROSWitmotionSensorController]: Initiating RTC pre-synchonization: current timestamp 2022-11-08T10:33:50.205
[INFO] [1667878430.205577692] [ROSWitmotionSensorController]: Configuration ROM: lock removal started
Instantiating timer at 30 ms
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x69
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x33
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x32
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x31
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x30
Configuration packet sent, flushing buffers...
Configuration completed
[INFO] [1667878435.207298822] [ROSWitmotionSensorController]: RTC pre-synchonization completed, saving configuration
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x0
Configuration packet sent, flushing buffers...
Configuration completed
[INFO] [1667878436.207643297] [ROSWitmotionSensorController]: RTC synchronized

comes from auto node = controller.Start(); and if we look at the next lines

[INFO] [1667878436.208435110] [MinimalPublisher]: QT spin !!!!!
[ERROR] [1667878436.208680980] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!
[INFO] [1667878436.208801294] [ROSWitmotionSensorController]: Entering SUSPENDED state
[INFO] [1667878436.208926868] [witmotion]: Publishing: 'Hello, world! '

We will see [ERROR] [1667878436.208680980] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate! then restart process is called since it is in SUSPENDED state[INFO] [1667878436.208801294] [ROSWitmotionSensorController]: Entering SUSPENDED state. The next line [INFO] [1667878436.208926868] [witmotion]: Publishing: 'Hello, world! ' is my publishing message from timer. It starts to publish thing so this statement

 std::thread spinThread([&executor, &app]() {
    executor.spin();

  });

  spinThread.detach();

works just fine. Also, from this output

[INFO] [1667878436.208435110] [MinimalPublisher]: QT spin !!!!!

app.exec(); should be executed. However, when restart process is start, there is some error

QObject::killTimer: Timers cannot be stopped from another thread
QObject::~QObject: Timers cannot be stopped from another thread
QSocketNotifier: Socket notifiers cannot be enabled or disabled from another thread
Suspending TTL connection, please emit RunPoll() again to proceed!
[ERROR] [1667878436.211071364] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!

and finally, it will get [ros2run]: Segmentation fault.

The problem is we cannot restart QTObject from ros2 node thread. I dont know how to solve this.

twdragon commented 1 year ago

@fllay well, now we have multithreaded schema working. The app.exec() call lives in the main thread, so the timer is stopped incorrectly due to a segmentation fault. The error message

[ERROR] [1667878436.211071364] [ROSWitmotionSensorController]: Sensor error: No data acquired during last 3 iterations, please check the baudrate!

you see is issued by witmotion::QBaseSerialWitmotionSensorReader object instance. In fact, it means that the serial port produces data but the reader object does not receive it. This fault is obviously reasoned to this fragment of code:

   reader->moveToThread(&reader_thread);
   connect(&reader_thread, &QThread::finished, reader, &QObject::deleteLater);
   connect(this, &ROSWitmotionSensorController::RunReader, reader, &QAbstractWitmotionSensorReader::RunPoll);
   connect(this, &ROSWitmotionSensorController::ConfigureSensor, reader, &QAbstractWitmotionSensorReader::SendConfig);
   connect(reader, &QAbstractWitmotionSensorReader::Acquired, this, &ROSWitmotionSensorController::Packet);
   connect(reader, &QAbstractWitmotionSensorReader::Error, this, &ROSWitmotionSensorController::Error);
   reader_thread.start();

It means that ReadData() slot is called at least thrice, so the program is attempting to read the data but no data feasible to decode is available. First, I can advise you to uncomment qt5_wrap_cpp(MOC_SOURCES include/witmotion_ros.h) line in CMakeLists.txt questioned above. Then: can you please send the parameters of the sensor you used to test it on ROS1 version of the package? I think, there is polling frequency / baudrate mismatch, due to this the data is produced, but the program reads garbage from the port.

P.S. Did you try to use witmotionctl- application in different modes?

fllay commented 1 year ago

First of all, it is my stupid mistake. When you ask me to get the parameter file from ros1, I swap the SD card and run ros1 node. It does not work so I check sensor connection and the power line was loose. After I fixed that issue, it still have a problem with polling frequency. Somehow, node->has_parameter("polling_interval") works differently in ros2. So no matter the polling_interval I set in yaml file, it will never been read and the polling_interval always 30ms. So I get rid of that line of code.

  //if (node->has_parameter("polling_interval")) {
    int int_interval;
    // node.param<int>("polling_interval", int_interval, 10);
    node->declare_parameter("polling_interval", 10);
    int_interval = node->get_parameter("polling_interval")
                       .get_parameter_value()
                       .get<int>();
    interval = static_cast<uint32_t>(int_interval);
    reader->SetSensorPollInterval(interval);
  //}

Now it works just fine.

pi@pi-desktop:~/ros2_ws$ ros2 run witmotion witmotion_ros_node --ros-args --params-file /home/pi/ros2_ws/src/witmotion_IMU_ros/config/wt61c.yml
[INFO] [1667915376.073052432] [ROSWitmotionSensorController]: Controller started
Opening device "ttyAMA1" at 115200 baud
[INFO] [1667915376.073475857] [ROSWitmotionSensorController]: Initiating RTC pre-synchonization: current timestamp 2022-11-08T20:49:36.073
[INFO] [1667915376.073541097] [ROSWitmotionSensorController]: Configuration ROM: lock removal started
Instantiating timer at 25 ms
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x69
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x33
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x32
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x31
Configuration packet sent, flushing buffers...
Configuration completed
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x30
Configuration packet sent, flushing buffers...
Configuration completed
[INFO] [1667915381.075317498] [ROSWitmotionSensorController]: RTC pre-synchonization completed, saving configuration
Configuration task detected, 1 commands in list, configuring sensor...
Sending configuration packet 0x0
Configuration packet sent, flushing buffers...
Configuration completed
[INFO] [1667915382.075676842] [ROSWitmotionSensorController]: RTC synchronized
[INFO] [1667915382.076343025] [MinimalPublisher]: QT spin !!!!!

and

average rate: 100.765
    min: 0.001s max: 0.025s std dev: 0.00981s window: 101
average rate: 100.373
    min: 0.001s max: 0.025s std dev: 0.00980s window: 201
average rate: 100.528
    min: 0.001s max: 0.025s std dev: 0.00979s window: 302

One problem now is rclcpp::shutdown(); does not work properly when the node is spin in thread. I will try to fixed that.

I will also do more test before you can make ROS2 branch and hopefully, someone who has other sensor model will test it. Thank you.

twdragon commented 1 year ago

@fllay thanks to you! Just let me know when your program passes the test, and then I will create the branch.

fllay commented 1 year ago

I fixed segmentation fault when press ctrl-c and edited both config and launch files according to ROS2. I think you can create the branch now. I tested it on wt61C only. I ordered W901c from ali-express but it will take a while to get it.

twdragon commented 1 year ago

@fllay I added the branch, we can now do the name fix, final review and merge.

ElettraSciComp / witmotion_IMU_ros

ros2 migration #12