myAGV，ROS2化，/cmd_vel

teleopを実行した後にtopic listを見ていたため/cmd_velがあるように見えていました．

(myenv) wataru@NucBoxG5:~$ docker run -it --rm \
  --net=host \
  --env RMW_IMPLEMENTATION=rmw_cyclonedds_cpp \
  osrf/ros:galactic-desktop
root@NucBoxG5:/# ros2 topic list
/accel/filtered
/diagnostics
/imu
/joint_states
/odom
/odometry/filtered
/parameter_events
/robot_description
/rosout
/scan
/set_pose
/tf
/tf_static
/voltage
/voltage_backup
/ydlidar_ros2_driver_node/transition_event

いないですね…．
起動ログを追いかけてみた所，

(ros2 galactic py3)er@er:~/myagv_ros2$ ros2 launch myagv_odometry myagv_active.launch.py
[INFO] [launch]: All log files can be found below /home/er/.ros/log/2026-05-22-09-40-24-525547-er-32658
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ydlidar_ros2_driver_node-7]: process started with pid [32698]
[INFO] [myagv_odometry_node-1]: process started with pid [32677]
...
[myagv_odometry_node-1] Serial buffer cleared successfully
...
[ERROR] [myagv_odometry_node-1]: process has died [pid 32893, exit code -6, cmd '/home/er/myagv_ros2/install/myagv_odometry/lib/myagv_odometry/myagv_odometry_node --ros-args -r __node:=myagv_odometry_node'].
...
[myagv_odometry_node-1] [INFO] [1779414400.608016034] [rclcpp]: signal_handler(signal_value=2)
[myagv_odometry_node-1] terminate called after throwing an instance of 'boost::wrapexcept<boost::system::system_error>'
[myagv_odometry_node-1]   what():  read: Interrupted system call
...

コケてますね…．

原因判明．
大きな原因は，ROS2のソースはjetson nano用でしたので，接続ポートが違うのと通信コマンドが異なっていた　という部分です．
差分は以下です．

diff --git a/myagv_odometry/src/myAGV.cpp b/myagv_odometry/src/myAGV.cpp
index d513bea..3f5d431 100755
--- a/myagv_odometry/src/myAGV.cpp
+++ b/myagv_odometry/src/myAGV.cpp
@@ -8,7 +8,7 @@
 const unsigned char header[2] = { 0xfe, 0xfe };
 
 boost::asio::io_service iosev;
-boost::asio::serial_port sp(iosev, "/dev/ttyS0");
+boost::asio::serial_port sp(iosev, "/dev/ttyAMA2");
 
 std::array<double, 36> odom_pose_covariance = {
     {1e-9, 0, 0, 0, 0, 0,
@@ -179,6 +179,7 @@ bool MyAGV::readSpeed()
     imu_data.angular_velocity.y = ((buf[index + 11] + buf[index + 12] * 256 ) - 10000) * 0.1;
     imu_data.angular_velocity.z = ((buf[index + 13] + buf[index + 14] * 256 ) - 10000) * 0.1;
 
+    /*
     uint8_t highNibble = (buf[index + 15] >> 4) & 0x0F;  
 
     Battery_voltage = (float)buf[index + 16] / 10.0f;
@@ -212,6 +213,7 @@ bool MyAGV::readSpeed()
                                         //  << imu_data.linear_acceleration.x << "," << imu_data.linear_acceleration.y << "," << imu_data.linear_acceleration.z << "|"
                                     //  << imu_data.angular_velocity.x << "," << imu_data.angular_velocity.y << "," << imu_data.angular_velocity.z << std::endl;
     //std::cout << "current pos is: " << x << "," << y << "," << theta << std::endl;
+    */
 
     return true;
 }
@@ -233,13 +235,13 @@ void MyAGV::writeSpeed(double movex, double movey, double rot)
     char buf[8] = { 0 };
     buf[0] = header[0];
     buf[1] = header[1];
-    buf[2] = move_cmd >> 8;
-    buf[3] = move_cmd & 0xff;
-    buf[4] = x_send;
-    buf[5] = y_send;
-    buf[6] = rot_send;
-    check = (buf[2] + buf[3] + buf[4] + buf[5] + buf[6]) & 0xff;
-    buf[7] = check;
+    // buf[2] = move_cmd >> 8;
+    // buf[3] = move_cmd & 0xff;
+    buf[2] = x_send;
+    buf[3] = y_send;
+    buf[4] = rot_send;
+    // check = (buf[2] + buf[3] + buf[4] + buf[5] + buf[6]) & 0xff;
+    buf[5] = check;
 
     boost::asio::write(sp, boost::asio::buffer(buf));
 }
@@ -341,7 +343,7 @@ void MyAGV::execute(double linearX, double linearY, double angularZ)
 {   
     currentTime = this->get_clock()->now();   
     double dt = (currentTime - lastTime).seconds();
-    if (true ==  readSpeed()) 
+    // if (true ==  readSpeed()) 
     {    
         writeSpeed(linearX, linearY, angularZ);
         publisherOdom(dt);
@@ -358,6 +360,7 @@ void MyAGV::execute(double linearX, double linearY, double angularZ)
  */
 void MyAGV::setAutoReportState(bool state)
 {
+    return;
     // Motor auto report
     unsigned char cmd[6] = {0xfe, 0xfe, 0x01, 0x0c, state, 0x0e};
     short cmd_sum = 0;
@@ -378,4 +381,4 @@ void MyAGV::setAutoReportState(bool state)
     boost::asio::write(sp, boost::asio::buffer(cmd));
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
     return;
-}
\ No newline at end of file
+}
diff --git a/myagv_odometry/src/myAGVSub.cpp b/myagv_odometry/src/myAGVSub.cpp
index 832cc4a..803cadb 100755
--- a/myagv_odometry/src/myAGVSub.cpp
+++ b/myagv_odometry/src/myAGVSub.cpp
@@ -41,4 +41,4 @@ int main(int argc, char* argv[])
 
     rclcpp::shutdown();
     return 0;
-}
\ No newline at end of file
+}
diff --git a/ydlidar_ros2_driver/params/X2.yaml b/ydlidar_ros2_driver/params/X2.yaml
index f36565a..38987b4 100644
--- a/ydlidar_ros2_driver/params/X2.yaml
+++ b/ydlidar_ros2_driver/params/X2.yaml
@@ -1,6 +1,7 @@
 ydlidar_ros2_driver_node:
   ros__parameters:
-    port: /dev/ttyTHS1
+    # port: /dev/ttyTHS1
+    port: /dev/ttyAMA0
     frame_id: laser_frame
     ignore_array: "-50,50"
     baudrate: 115200