spinonce时，无法正常订阅话题
-
最近想写一个通过简单的串口协议，遇到了一点问题。我的主函数需要不断循环接收串口，我采用了while循环+spinonce的方式执行。然后我还需要将其他信息发送出去。我使用subcribe订阅话题+回调函数的方式发送，但是我的subscriber似乎没有正确订阅，因为我使用teleop_twist_keyboard节点发送cmd_vel时显示话题没有被订阅。以下是我的主函数代码:
int main(int argc, char **argv) { serial::Serial ser; std::string tf_parent_frame_id; std::string tf_frame_id; std::string frame_id; double time_offset_in_seconds; bool broadcast_tf; double linear_acceleration_stddev; double angular_velocity_stddev; double orientation_stddev; uint8_t last_received_message_number; bool received_message = false; int data_packet_start; tf::Quaternion orientation; tf::Quaternion zero_orientation; ros::init(argc, argv, "mpu6050_serial_ttl_to_imu_node"); ros::NodeHandle private_node_handle("~"); private_node_handle.param<std::string>("port", port, "/dev/ttyUSB0"); // USB端口 private_node_handle.param<std::string>("tf_parent_frame_id", tf_parent_frame_id, "imu_base"); private_node_handle.param<std::string>("tf_frame_id", tf_frame_id, "imu_link"); private_node_handle.param<std::string>("frame_id", frame_id, "imu_link"); private_node_handle.param<double>("time_offset_in_seconds", time_offset_in_seconds, 0.0); private_node_handle.param<bool>("broadcast_tf", broadcast_tf, true); // Standard Deviation,StdDev,标准偏差指标,目的是用来衡量**的波动性 private_node_handle.param<double>("linear_acceleration_stddev", linear_acceleration_stddev, 0.0); private_node_handle.param<double>("angular_velocity_stddev", angular_velocity_stddev, 0.0); private_node_handle.param<double>("orientation_stddev", orientation_stddev, 0.0); ros::NodeHandle nh("imu"); ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("data", 100); ros::Publisher imu_temperature_pub = nh.advertise<sensor_msgs::Temperature>("temperature", 50); ros::ServiceServer service = nh.advertiseService("set_zero_orientation", set_zero_orientation); // 定义了一个服务器 ros::Subscriber sub = nh.subscribe("cmd_vel", 20, callback); ros::Rate r(200); // 200 hz sensor_msgs::Imu imu; // 标准数据类型 可见 http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html /** * Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance # Row major about x, y, z axes geometry_msgs/Vector3 angular_velocity float64[9] angular_velocity_covariance # Row major about x, y, z axes geometry_msgs/Vector3 linear_acceleration float64[9] linear_acceleration_covariance # Row major x, y z **/ imu.linear_acceleration_covariance[0] = linear_acceleration_stddev; // 线性加速度协方差 = 线性加速度标准差? imu.linear_acceleration_covariance[4] = linear_acceleration_stddev; imu.linear_acceleration_covariance[8] = linear_acceleration_stddev; imu.angular_velocity_covariance[0] = angular_velocity_stddev; imu.angular_velocity_covariance[4] = angular_velocity_stddev; imu.angular_velocity_covariance[8] = angular_velocity_stddev; imu.orientation_covariance[0] = orientation_stddev; imu.orientation_covariance[4] = orientation_stddev; imu.orientation_covariance[8] = orientation_stddev; /** * Header header # timestamp is the time the temperature was measured # frame_id is the location of the temperature reading float64 temperature # Measurement of the Temperature in Degrees Celsius float64 variance # 0 is interpreted as variance unknown 方差 **/ sensor_msgs::Temperature temperature_msg; temperature_msg.variance = 0; //方差 static tf::TransformBroadcaster tf_br; tf::Transform transform; transform.setOrigin(tf::Vector3(0, 0, 0)); std::string input; std::string read; while (ros::ok()) { try { if (ser.isOpen()) // 端口打开 { // read string from serial device 从串行设备读取字符串 if (ser.available()) { read = ser.read(ser.available()); // ROS_INFO("read %i new characters from serial port, adding to %i characters of old input.", (int)read.size(), (int)input.size()); input += read; // ROS_WARN_STREAM(" Wait read "); // 读到的字符串长度>=33 输入中可能有完整的包 while (input.length() >= 35) // while there might be a complete package in input { //parse for data packets 解析数据包 data_packet_start = input.find("\x55\x51"); // 找开头的标记位 if (data_packet_start != std::string::npos) { ROS_DEBUG("found possible start of data packet at position %d", data_packet_start); if ((input.length() >= data_packet_start + 33) && (input.compare(data_packet_start + 11, 2, "\x55\x52") == 0) && (input.compare(data_packet_start + 22, 2, "\x55\x53") == 0)) //check if positions 26,27 exist, then test values { // ROS_ERROR_STREAM(" Found "); // ROS_INFO("Found"); /** * 采用和陀螺仪同样的计算方法，当AFS_SEL=3时，数字-32767对应-16g，32767对应16g。把32767除以16，就可以得到2048， 即我们说的灵敏度。把从加速度计读出的数字除以2048，就可以换算成加速度的数值。举个例子，如果我们从加速度计读到的数字是1000，那么对应的加速度数据是1000/2048=0.49g。g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。 * * 也就是说说直接从imu中读取到的并不是真正的加速度值，而是一个比例值，相当于几倍的重力加速度g * **/ // get gyro values 获得陀螺值 ROS_DEBUG("seems to be a real data package: long enough and found end characters"); for(int i = 0;i<3;i++){ vel_angular[0].data[1+i] = input[data_packet_start+2+i]; vel_angular[1].data[1+i] = input[data_packet_start+5+i]; vel_angular[2].data[1+i] = input[data_packet_start+8+i]; accel[0].data[1+i] = input[data_packet_start+13+i]; accel[1].data[1+i] = input[data_packet_start+16+i]; accel[2].data[1+i] = input[data_packet_start+19+i]; yaw.data[1+i] = input[data_packet_start+24+i]; roll.data[1+i] = input[data_packet_start+27+i]; pitch.data[1+i] = input[data_packet_start+30+i]; } ROS_INFO("%f,%f,%f",yaw.d,roll.d,pitch.d); tf::Matrix3x3 obs_mat; obs_mat.setEulerYPR(yaw.d, pitch.d, roll.d); tf::Quaternion orientation; obs_mat.getRotation(orientation); // calculate measurement time if (!zero_orientation_set) { zero_orientation = orientation; zero_orientation_set = true; } //http://answers.ros.org/question/10124/relative-rotation-between-two-quaternions/ tf::Quaternion differential_rotation; differential_rotation = zero_orientation.inverse() * orientation; ros::Time measurement_time = ros::Time::now() + ros::Duration(time_offset_in_seconds); // publish imu message imu.header.stamp = measurement_time; imu.header.frame_id = frame_id; // quaternionTFToMsg(differential_rotation, imu.orientation); // 把tf四元数转化为geomsg四元数 imu.angular_velocity.x = vel_angular[0].d; // 角速度 imu.angular_velocity.y = vel_angular[1].d; imu.angular_velocity.z = vel_angular[2].d; imu.linear_acceleration.x = accel[0].d; // 线加速度 imu.linear_acceleration.y = accel[1].d; imu.linear_acceleration.z = accel[2].d; imu_pub.publish(imu); quaternionTFToMsg(differential_rotation, imu.orientation); // publish temperature message // publish tf transform if (broadcast_tf) { transform.setRotation(differential_rotation); tf_br.sendTransform(tf::StampedTransform(transform, measurement_time, tf_parent_frame_id, tf_frame_id)); } input.erase(0, data_packet_start + 33); // delete everything up to and including the processed packet } else { // ROS_INFO("%s",input.substr(data_packet_start + 22, 2).c_str()); // ROS_INFO("%d",(input.length()-data_packet_start)); if (input.length() >= data_packet_start + 33) { input.erase(0, data_packet_start + 1); // delete up to false data_packet_start character so it is not found again } else { // do not delete start character, maybe complete package has not arrived yet input.erase(0, data_packet_start); } } } else { // no start character found in input, so delete everything input.clear(); } } } } else { // try and open the serial port try { ser.setPort(port); ser.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); ser.setTimeout(to); ser.open(); } catch (serial::IOException &e) { ROS_ERROR_STREAM("Unable to open serial port " << ser.getPort() << ". Trying again in 5 seconds."); ros::Duration(5).sleep(); } if (ser.isOpen()) { ROS_DEBUG_STREAM("Serial port " << ser.getPort() << " initialized and opened."); } } } catch (serial::IOException &e) { ROS_ERROR_STREAM("Error reading from the serial port " << ser.getPort() << ". Closing connection."); ser.close(); } ros::spinOnce(); r.sleep(); } ros::spin(); return 0; }是因为有什么问题呢，或者有什么更好的方式进行通讯。
我的环境是ros noetic -
@2429148505 在 spinonce时，无法正常订阅话题 中说：
最近想写一个通过简单的串口协议，遇到了一点问题。我的主函数需要不断循环接收串口，我采用了while循环+spinonce的方式执行。然后我还需要将其他信息发送出去。我使用subcribe订阅话题+回调函数的方式发送，但是我的subscriber似乎没有正确订阅，因为我使用teleop_twist_keyboard节点发送cmd_vel时显示话题没有被订阅。以下是我的主函数代码:
int main(int argc, char **argv) { serial::Serial ser; std::string tf_parent_frame_id; std::string tf_frame_id; std::string frame_id; double time_offset_in_seconds; bool broadcast_tf; double linear_acceleration_stddev; double angular_velocity_stddev; double orientation_stddev; uint8_t last_received_message_number; bool received_message = false; int data_packet_start; tf::Quaternion orientation; tf::Quaternion zero_orientation; ros::init(argc, argv, "mpu6050_serial_ttl_to_imu_node"); ros::NodeHandle private_node_handle("~"); private_node_handle.param<std::string>("port", port, "/dev/ttyUSB0"); // USB端口 private_node_handle.param<std::string>("tf_parent_frame_id", tf_parent_frame_id, "imu_base"); private_node_handle.param<std::string>("tf_frame_id", tf_frame_id, "imu_link"); private_node_handle.param<std::string>("frame_id", frame_id, "imu_link"); private_node_handle.param<double>("time_offset_in_seconds", time_offset_in_seconds, 0.0); private_node_handle.param<bool>("broadcast_tf", broadcast_tf, true); // Standard Deviation,StdDev,标准偏差指标,目的是用来衡量**的波动性 private_node_handle.param<double>("linear_acceleration_stddev", linear_acceleration_stddev, 0.0); private_node_handle.param<double>("angular_velocity_stddev", angular_velocity_stddev, 0.0); private_node_handle.param<double>("orientation_stddev", orientation_stddev, 0.0); ros::NodeHandle nh("imu"); ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("data", 100); ros::Publisher imu_temperature_pub = nh.advertise<sensor_msgs::Temperature>("temperature", 50); ros::ServiceServer service = nh.advertiseService("set_zero_orientation", set_zero_orientation); // 定义了一个服务器 ros::Subscriber sub = nh.subscribe("cmd_vel", 20, callback); ros::Rate r(200); // 200 hz sensor_msgs::Imu imu; // 标准数据类型 可见 http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html /** * Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance # Row major about x, y, z axes geometry_msgs/Vector3 angular_velocity float64[9] angular_velocity_covariance # Row major about x, y, z axes geometry_msgs/Vector3 linear_acceleration float64[9] linear_acceleration_covariance # Row major x, y z **/ imu.linear_acceleration_covariance[0] = linear_acceleration_stddev; // 线性加速度协方差 = 线性加速度标准差? imu.linear_acceleration_covariance[4] = linear_acceleration_stddev; imu.linear_acceleration_covariance[8] = linear_acceleration_stddev; imu.angular_velocity_covariance[0] = angular_velocity_stddev; imu.angular_velocity_covariance[4] = angular_velocity_stddev; imu.angular_velocity_covariance[8] = angular_velocity_stddev; imu.orientation_covariance[0] = orientation_stddev; imu.orientation_covariance[4] = orientation_stddev; imu.orientation_covariance[8] = orientation_stddev; /** * Header header # timestamp is the time the temperature was measured # frame_id is the location of the temperature reading float64 temperature # Measurement of the Temperature in Degrees Celsius float64 variance # 0 is interpreted as variance unknown 方差 **/ sensor_msgs::Temperature temperature_msg; temperature_msg.variance = 0; //方差 static tf::TransformBroadcaster tf_br; tf::Transform transform; transform.setOrigin(tf::Vector3(0, 0, 0)); std::string input; std::string read; while (ros::ok()) { try { if (ser.isOpen()) // 端口打开 { // read string from serial device 从串行设备读取字符串 if (ser.available()) { read = ser.read(ser.available()); // ROS_INFO("read %i new characters from serial port, adding to %i characters of old input.", (int)read.size(), (int)input.size()); input += read; // ROS_WARN_STREAM(" Wait read "); // 读到的字符串长度>=33 输入中可能有完整的包 while (input.length() >= 35) // while there might be a complete package in input { //parse for data packets 解析数据包 data_packet_start = input.find("\x55\x51"); // 找开头的标记位 if (data_packet_start != std::string::npos) { ROS_DEBUG("found possible start of data packet at position %d", data_packet_start); if ((input.length() >= data_packet_start + 33) && (input.compare(data_packet_start + 11, 2, "\x55\x52") == 0) && (input.compare(data_packet_start + 22, 2, "\x55\x53") == 0)) //check if positions 26,27 exist, then test values { // ROS_ERROR_STREAM(" Found "); // ROS_INFO("Found"); /** * 采用和陀螺仪同样的计算方法，当AFS_SEL=3时，数字-32767对应-16g，32767对应16g。把32767除以16，就可以得到2048， 即我们说的灵敏度。把从加速度计读出的数字除以2048，就可以换算成加速度的数值。举个例子，如果我们从加速度计读到的数字是1000，那么对应的加速度数据是1000/2048=0.49g。g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。 * * 也就是说说直接从imu中读取到的并不是真正的加速度值，而是一个比例值，相当于几倍的重力加速度g * **/ // get gyro values 获得陀螺值 ROS_DEBUG("seems to be a real data package: long enough and found end characters"); for(int i = 0;i<3;i++){ vel_angular[0].data[1+i] = input[data_packet_start+2+i]; vel_angular[1].data[1+i] = input[data_packet_start+5+i]; vel_angular[2].data[1+i] = input[data_packet_start+8+i]; accel[0].data[1+i] = input[data_packet_start+13+i]; accel[1].data[1+i] = input[data_packet_start+16+i]; accel[2].data[1+i] = input[data_packet_start+19+i]; yaw.data[1+i] = input[data_packet_start+24+i]; roll.data[1+i] = input[data_packet_start+27+i]; pitch.data[1+i] = input[data_packet_start+30+i]; } ROS_INFO("%f,%f,%f",yaw.d,roll.d,pitch.d); tf::Matrix3x3 obs_mat; obs_mat.setEulerYPR(yaw.d, pitch.d, roll.d); tf::Quaternion orientation; obs_mat.getRotation(orientation); // calculate measurement time if (!zero_orientation_set) { zero_orientation = orientation; zero_orientation_set = true; } //http://answers.ros.org/question/10124/relative-rotation-between-two-quaternions/ tf::Quaternion differential_rotation; differential_rotation = zero_orientation.inverse() * orientation; ros::Time measurement_time = ros::Time::now() + ros::Duration(time_offset_in_seconds); // publish imu message imu.header.stamp = measurement_time; imu.header.frame_id = frame_id; // quaternionTFToMsg(differential_rotation, imu.orientation); // 把tf四元数转化为geomsg四元数 imu.angular_velocity.x = vel_angular[0].d; // 角速度 imu.angular_velocity.y = vel_angular[1].d; imu.angular_velocity.z = vel_angular[2].d; imu.linear_acceleration.x = accel[0].d; // 线加速度 imu.linear_acceleration.y = accel[1].d; imu.linear_acceleration.z = accel[2].d; imu_pub.publish(imu); quaternionTFToMsg(differential_rotation, imu.orientation); // publish temperature message // publish tf transform if (broadcast_tf) { transform.setRotation(differential_rotation); tf_br.sendTransform(tf::StampedTransform(transform, measurement_time, tf_parent_frame_id, tf_frame_id)); } input.erase(0, data_packet_start + 33); // delete everything up to and including the processed packet } else { // ROS_INFO("%s",input.substr(data_packet_start + 22, 2).c_str()); // ROS_INFO("%d",(input.length()-data_packet_start)); if (input.length() >= data_packet_start + 33) { input.erase(0, data_packet_start + 1); // delete up to false data_packet_start character so it is not found again } else { // do not delete start character, maybe complete package has not arrived yet input.erase(0, data_packet_start); } } } else { // no start character found in input, so delete everything input.clear(); } } } } else { // try and open the serial port try { ser.setPort(port); ser.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); ser.setTimeout(to); ser.open(); } catch (serial::IOException &e) { ROS_ERROR_STREAM("Unable to open serial port " << ser.getPort() << ". Trying again in 5 seconds."); ros::Duration(5).sleep(); } if (ser.isOpen()) { ROS_DEBUG_STREAM("Serial port " << ser.getPort() << " initialized and opened."); } } } catch (serial::IOException &e) { ROS_ERROR_STREAM("Error reading from the serial port " << ser.getPort() << ". Closing connection."); ser.close(); } ros::spinOnce(); r.sleep(); } ros::spin(); return 0; }是因为有什么问题呢，或者有什么更好的方式进行通讯。
我的环境是ros noetic解决了，是node后面加了(imu)，导致话题接收在/imu/cmd_vel下了
-
在您的代码中，您使用了
ros::spinOnce()来处理ROS事件，但是您也在主循环中执行了长时间运行的操作（读取串口数据和处理数据包），这可能导致ros::spinOnce()没有足够的机会来检查和处理订阅的消息。当您调用
ros::spinOnce()时，它会检查并调用所有等待的回调函数（例如，订阅的回调）。然而，如果您的主循环阻塞了很长时间，那么这些回调可能不会被及时调用。此外，您的代码在循环的最后调用了
ros::spin()，这是不必要的，因为您已经在循环中使用了ros::spinOnce()。调用ros::spin()会进入一个新的循环，这可能导致不可预测的行为，因为您已经有一个循环在处理串口数据和ROS事件。为了解决这个问题，您可以尝试以下策略：
- 增加
ros::spinOnce()的调用频率：您可以通过减少ros::Rate r(200);中的频率来增加ros::spinOnce()的调用次数。但是，请注意，这可能会增加CPU的使用率。 - 使用ROS的异步回调：对于串口读取和处理，您可以考虑使用ROS的异步回调功能，而不是在主循环中直接处理。这样，您就可以让ROS回调机制来处理消息，而不必在主循环中阻塞。
- 分离串口处理和ROS事件处理：您可以将串口读取和处理放在一个单独的线程中，以便它可以并行运行而不阻塞ROS事件的处理。这将需要一些线程同步的知识，但可以使您的程序更加响应和高效。
- 使用ROS的Serial库：ROS提供了一个用于串口通信的库（
rosserial），它可能更适合在ROS环境中使用。虽然它可能不直接支持您的特定硬件或协议，但它提供了一个框架，您可以基于它进行扩展。
最后，请注意，您的代码中存在一些可能的错误或不一致之处，例如使用
<和>而不是<和>来表示小于和大于符号，以及private_node_handle.param<std::string>应该是private_node_handle.param<std::string>("key", value, default_value);。确保您的代码没有这些错误，并正确设置了所有必要的参数。
以上为生成回答，仅供参考～
- 增加
-
@2429148505 在 spinonce时，无法正常订阅话题 中说：
最近想写一个通过简单的串口协议，遇到了一点问题。我的主函数需要不断循环接收串口，我采用了while循环+spinonce的方式执行。然后我还需要将其他信息发送出去。我使用subcribe订阅话题+回调函数的方式发送，但是我的subscriber似乎没有正确订阅，因为我使用teleop_twist_keyboard节点发送cmd_vel时显示话题没有被订阅。以下是我的主函数代码:
int main(int argc, char **argv) { serial::Serial ser; std::string tf_parent_frame_id; std::string tf_frame_id; std::string frame_id; double time_offset_in_seconds; bool broadcast_tf; double linear_acceleration_stddev; double angular_velocity_stddev; double orientation_stddev; uint8_t last_received_message_number; bool received_message = false; int data_packet_start; tf::Quaternion orientation; tf::Quaternion zero_orientation; ros::init(argc, argv, "mpu6050_serial_ttl_to_imu_node"); ros::NodeHandle private_node_handle("~"); private_node_handle.param<std::string>("port", port, "/dev/ttyUSB0"); // USB端口 private_node_handle.param<std::string>("tf_parent_frame_id", tf_parent_frame_id, "imu_base"); private_node_handle.param<std::string>("tf_frame_id", tf_frame_id, "imu_link"); private_node_handle.param<std::string>("frame_id", frame_id, "imu_link"); private_node_handle.param<double>("time_offset_in_seconds", time_offset_in_seconds, 0.0); private_node_handle.param<bool>("broadcast_tf", broadcast_tf, true); // Standard Deviation,StdDev,标准偏差指标,目的是用来衡量**的波动性 private_node_handle.param<double>("linear_acceleration_stddev", linear_acceleration_stddev, 0.0); private_node_handle.param<double>("angular_velocity_stddev", angular_velocity_stddev, 0.0); private_node_handle.param<double>("orientation_stddev", orientation_stddev, 0.0); ros::NodeHandle nh("imu"); ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("data", 100); ros::Publisher imu_temperature_pub = nh.advertise<sensor_msgs::Temperature>("temperature", 50); ros::ServiceServer service = nh.advertiseService("set_zero_orientation", set_zero_orientation); // 定义了一个服务器 ros::Subscriber sub = nh.subscribe("cmd_vel", 20, callback); ros::Rate r(200); // 200 hz sensor_msgs::Imu imu; // 标准数据类型 可见 http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html /** * Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance # Row major about x, y, z axes geometry_msgs/Vector3 angular_velocity float64[9] angular_velocity_covariance # Row major about x, y, z axes geometry_msgs/Vector3 linear_acceleration float64[9] linear_acceleration_covariance # Row major x, y z **/ imu.linear_acceleration_covariance[0] = linear_acceleration_stddev; // 线性加速度协方差 = 线性加速度标准差? imu.linear_acceleration_covariance[4] = linear_acceleration_stddev; imu.linear_acceleration_covariance[8] = linear_acceleration_stddev; imu.angular_velocity_covariance[0] = angular_velocity_stddev; imu.angular_velocity_covariance[4] = angular_velocity_stddev; imu.angular_velocity_covariance[8] = angular_velocity_stddev; imu.orientation_covariance[0] = orientation_stddev; imu.orientation_covariance[4] = orientation_stddev; imu.orientation_covariance[8] = orientation_stddev; /** * Header header # timestamp is the time the temperature was measured # frame_id is the location of the temperature reading float64 temperature # Measurement of the Temperature in Degrees Celsius float64 variance # 0 is interpreted as variance unknown 方差 **/ sensor_msgs::Temperature temperature_msg; temperature_msg.variance = 0; //方差 static tf::TransformBroadcaster tf_br; tf::Transform transform; transform.setOrigin(tf::Vector3(0, 0, 0)); std::string input; std::string read; while (ros::ok()) { try { if (ser.isOpen()) // 端口打开 { // read string from serial device 从串行设备读取字符串 if (ser.available()) { read = ser.read(ser.available()); // ROS_INFO("read %i new characters from serial port, adding to %i characters of old input.", (int)read.size(), (int)input.size()); input += read; // ROS_WARN_STREAM(" Wait read "); // 读到的字符串长度>=33 输入中可能有完整的包 while (input.length() >= 35) // while there might be a complete package in input { //parse for data packets 解析数据包 data_packet_start = input.find("\x55\x51"); // 找开头的标记位 if (data_packet_start != std::string::npos) { ROS_DEBUG("found possible start of data packet at position %d", data_packet_start); if ((input.length() >= data_packet_start + 33) && (input.compare(data_packet_start + 11, 2, "\x55\x52") == 0) && (input.compare(data_packet_start + 22, 2, "\x55\x53") == 0)) //check if positions 26,27 exist, then test values { // ROS_ERROR_STREAM(" Found "); // ROS_INFO("Found"); /** * 采用和陀螺仪同样的计算方法，当AFS_SEL=3时，数字-32767对应-16g，32767对应16g。把32767除以16，就可以得到2048， 即我们说的灵敏度。把从加速度计读出的数字除以2048，就可以换算成加速度的数值。举个例子，如果我们从加速度计读到的数字是1000，那么对应的加速度数据是1000/2048=0.49g。g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。 * * 也就是说说直接从imu中读取到的并不是真正的加速度值，而是一个比例值，相当于几倍的重力加速度g * **/ // get gyro values 获得陀螺值 ROS_DEBUG("seems to be a real data package: long enough and found end characters"); for(int i = 0;i<3;i++){ vel_angular[0].data[1+i] = input[data_packet_start+2+i]; vel_angular[1].data[1+i] = input[data_packet_start+5+i]; vel_angular[2].data[1+i] = input[data_packet_start+8+i]; accel[0].data[1+i] = input[data_packet_start+13+i]; accel[1].data[1+i] = input[data_packet_start+16+i]; accel[2].data[1+i] = input[data_packet_start+19+i]; yaw.data[1+i] = input[data_packet_start+24+i]; roll.data[1+i] = input[data_packet_start+27+i]; pitch.data[1+i] = input[data_packet_start+30+i]; } ROS_INFO("%f,%f,%f",yaw.d,roll.d,pitch.d); tf::Matrix3x3 obs_mat; obs_mat.setEulerYPR(yaw.d, pitch.d, roll.d); tf::Quaternion orientation; obs_mat.getRotation(orientation); // calculate measurement time if (!zero_orientation_set) { zero_orientation = orientation; zero_orientation_set = true; } //http://answers.ros.org/question/10124/relative-rotation-between-two-quaternions/ tf::Quaternion differential_rotation; differential_rotation = zero_orientation.inverse() * orientation; ros::Time measurement_time = ros::Time::now() + ros::Duration(time_offset_in_seconds); // publish imu message imu.header.stamp = measurement_time; imu.header.frame_id = frame_id; // quaternionTFToMsg(differential_rotation, imu.orientation); // 把tf四元数转化为geomsg四元数 imu.angular_velocity.x = vel_angular[0].d; // 角速度 imu.angular_velocity.y = vel_angular[1].d; imu.angular_velocity.z = vel_angular[2].d; imu.linear_acceleration.x = accel[0].d; // 线加速度 imu.linear_acceleration.y = accel[1].d; imu.linear_acceleration.z = accel[2].d; imu_pub.publish(imu); quaternionTFToMsg(differential_rotation, imu.orientation); // publish temperature message // publish tf transform if (broadcast_tf) { transform.setRotation(differential_rotation); tf_br.sendTransform(tf::StampedTransform(transform, measurement_time, tf_parent_frame_id, tf_frame_id)); } input.erase(0, data_packet_start + 33); // delete everything up to and including the processed packet } else { // ROS_INFO("%s",input.substr(data_packet_start + 22, 2).c_str()); // ROS_INFO("%d",(input.length()-data_packet_start)); if (input.length() >= data_packet_start + 33) { input.erase(0, data_packet_start + 1); // delete up to false data_packet_start character so it is not found again } else { // do not delete start character, maybe complete package has not arrived yet input.erase(0, data_packet_start); } } } else { // no start character found in input, so delete everything input.clear(); } } } } else { // try and open the serial port try { ser.setPort(port); ser.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); ser.setTimeout(to); ser.open(); } catch (serial::IOException &e) { ROS_ERROR_STREAM("Unable to open serial port " << ser.getPort() << ". Trying again in 5 seconds."); ros::Duration(5).sleep(); } if (ser.isOpen()) { ROS_DEBUG_STREAM("Serial port " << ser.getPort() << " initialized and opened."); } } } catch (serial::IOException &e) { ROS_ERROR_STREAM("Error reading from the serial port " << ser.getPort() << ". Closing connection."); ser.close(); } ros::spinOnce(); r.sleep(); } ros::spin(); return 0; }是因为有什么问题呢，或者有什么更好的方式进行通讯。
我的环境是ros noetic解决了，是node后面加了(imu)，导致话题接收在/imu/cmd_vel下了
-
小 小鱼 将这个主题标记为已解决，在
