欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

ROS : IMU串口通讯接口(通用版)

程序员文章站 2022-04-18 16:46:29
...
1、源码
  1. #include <string>
  2. #include <ros/ros.h> // 包含ROS的头文件
  3. #include <sensor_msgs/JointState.h>
  4. #include <tf/transform_broadcaster.h>
  5. #include <nav_msgs/Odometry.h>
  6. #include <boost/asio.hpp> //包含boost库函数
  7. #include <boost/bind.hpp>
  8. #include <math.h>
  9. #include "std_msgs/String.h" //ros定义的String数据类型
  10. #include <std_msgs/Float32.h>
  11. using namespace std;
  12. using namespace boost::asio; //定义一个命名空间,用于后面的读写操作
  13. unsigned char buf[17]; //定义字符串长度,IMU返回的数据是17个字节一组,可用串口调试助手获得
  14. std::string string_to_hex(const std::string& input)
  15. {
  16. static const char* const lut = "0123456789ABCDEF";
  17. size_t len = input.length();
  18. std::string output;
  19. output.reserve(2 * len);
  20. for (size_t i = 0; i < len; ++i)
  21. {
  22. const unsigned char c = input[i];
  23. output.push_back(lut[c >> 4]);
  24. output.push_back(lut[c & 15]);
  25. }
  26. return output;
  27. }
  28. int main(int argc, char** argv)
  29. {
  30. ros::init(argc, argv, "boost"); //初始化节点
  31. ros::NodeHandle n;
  32. ros::Publisher IMU_pub = n.advertise<std_msgs::Float32>("IMU_data", 1000); //定义发布消息的名称及sulv
  33. ros::Rate loop_rate(100);
  34. io_service iosev;
  35. serial_port sp(iosev, "/dev/ttyUSB0"); //定义传输的串口
  36. sp.set_option(serial_port::baud_rate(115200));
  37. sp.set_option(serial_port::flow_control());
  38. sp.set_option(serial_port::parity());
  39. sp.set_option(serial_port::stop_bits());
  40. sp.set_option(serial_port::character_size(8));
  41. while (ros::ok())
  42. {
  43. read (sp,buffer(buf));
  44. string str(&buf[0],&buf[17]); //将数组转化为字符串
  45. std_msgs::String msg;
  46. std::stringstream ss;
  47. ss << str;
  48. std_msgs::Float32 Yaw; //定义机器人偏航角,单位为度数
  49. char higher;
  50. char lower;
  51. higher = buf[5];
  52. lower = buf[4];
  53. Yaw.data= (float)((higher * 256 + lower))/100;
  54. cout << Yaw << endl; //输出偏航角到终端
  55. msg.data = string_to_hex(ss.str());
  56. ROS_INFO("%s", msg.data.c_str()); //打印接受到的字符串
  57. IMU_pub.publish(Yaw); //发布消息
  58. ros::spinOnce();
  59. loop_rate.sleep();
  60. }
  61. iosev.run();
  62. return 0;
  63. }


2、结果

新终端,执行

$ roscore


最后我们把IMU通过USB口连接装好ROS的Ubuntu14.04的系统的PC,在Qt下编译无错误后,那么我们在Qt下运行就会输出偏航角Yaw的信息,同时发布topic;

ROS : IMU串口通讯接口(通用版)


如果仍然有错误提示/dev/ttyUSB0  拒绝访问,此时我们就需要对USB的权限进行设置,可以输入

 

  1. $ sudo chmod 666 /dev/ttyUSB0
  2. 或者
  3. $ sudo chmod a+rw /dev/ttyUSB0

解决/dev/ttyUSB0 denied permission永久解决方案

sudo usermod -aG dialout wsh

重启电脑即可
相关标签: ROS imu