ros2 migration #12

fllay · 2022-10-23T02:49:45Z

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 · 2022-10-23T16:57:04Z

@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 · 2022-11-03T07:25:50Z

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

twdragon · 2022-11-04T19:23:13Z

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

It seems working.

twdragon · 2022-11-04T19:25:19Z

@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 · 2022-11-05T02:31:55Z

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 · 2022-11-07T10:22:29Z

@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 · 2022-11-07T13:26:05Z

@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 · 2022-11-07T14:48:48Z

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 · 2022-11-07T15:28:29Z

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 · 2022-11-08T03:42:35Z

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 · 2022-11-08T10:03:29Z

@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 · 2022-11-08T14:01:43Z

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 · 2022-11-08T14:15:01Z

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

fllay · 2022-11-11T07:50:30Z

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

It is needed to change the project name back to witmotion_ros, then the updated PR will be merged into ros2 branch.

CMakeLists.txt

twdragon · 2022-11-11T16:00:32Z

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

ros2 code

8c8d3d8

Fix link error

ad498cb

twdragon linked an issue Nov 4, 2022 that may be closed by this pull request

any plan to support ros2? #7

Closed

add MultiThreadedExecutor in witmotion_ros_node

a0e1b35

fixed polling interval and threading

f4527d7

fllay added 2 commits November 10, 2022 22:49

fixed segmentation fault when Cttl-C

30ee2b2

add ROS2 launch files

6ef7abf

twdragon requested changes Nov 11, 2022

View reviewed changes

CMakeLists.txt Show resolved Hide resolved

CMakeLists.txt Show resolved Hide resolved

CMakeLists.txt Outdated Show resolved Hide resolved

CMakeLists.txt Outdated Show resolved Hide resolved

twdragon changed the base branch from main to ros2 November 11, 2022 15:59

Rename the project to witmotion_ros

793fe53

twdragon approved these changes Nov 14, 2022

View reviewed changes

twdragon merged commit b2692ec into ElettraSciComp:ros2 Nov 14, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ros2 migration #12

ros2 migration #12

fllay commented Oct 23, 2022

twdragon commented Oct 23, 2022

fllay commented Nov 3, 2022 •

edited

Loading

twdragon commented Nov 4, 2022

twdragon commented Nov 4, 2022

fllay commented Nov 5, 2022 •

edited by twdragon

Loading

twdragon commented Nov 7, 2022 •

edited

Loading

twdragon commented Nov 7, 2022

fllay commented Nov 7, 2022

twdragon commented Nov 7, 2022

fllay commented Nov 8, 2022 •

edited

Loading

twdragon commented Nov 8, 2022 •

edited

Loading

fllay commented Nov 8, 2022

twdragon commented Nov 8, 2022

fllay commented Nov 11, 2022

twdragon left a comment

twdragon commented Nov 11, 2022

ros2 migration #12

ros2 migration #12

Conversation

fllay commented Oct 23, 2022

twdragon commented Oct 23, 2022

fllay commented Nov 3, 2022 • edited Loading

twdragon commented Nov 4, 2022

twdragon commented Nov 4, 2022

fllay commented Nov 5, 2022 • edited by twdragon Loading

twdragon commented Nov 7, 2022 • edited Loading

twdragon commented Nov 7, 2022

fllay commented Nov 7, 2022

twdragon commented Nov 7, 2022

fllay commented Nov 8, 2022 • edited Loading

twdragon commented Nov 8, 2022 • edited Loading

fllay commented Nov 8, 2022

twdragon commented Nov 8, 2022

fllay commented Nov 11, 2022

twdragon left a comment

Choose a reason for hiding this comment

twdragon commented Nov 11, 2022

fllay commented Nov 3, 2022 •

edited

Loading

fllay commented Nov 5, 2022 •

edited by twdragon

Loading

twdragon commented Nov 7, 2022 •

edited

Loading

fllay commented Nov 8, 2022 •

edited

Loading

twdragon commented Nov 8, 2022 •

edited

Loading