JY901在ubuntu18.04+ROS下的使用

JY901在ubuntu18.04+ROS下的使用

一、JY901与Arduino mega2560物理连接

四线连接

Arduino	JY901
5V	VCC
GND	GND
TX1	RX
RX1	TX

二、烧录程序

将JY901SerialMega2560示例包粘贴到libraries中
下载地址：JY901SerialMega2560
打开IDE，在example中找到JY901Serial
编译->烧录
如果烧录不成功，尝试打开串口

sudo chmod a+rw /dev/ttyUSB0

成功后打开串口监视器

三、改动代码，创建ROS节点

在arduino IDE中写入以下代码并编译上传

#include <Wire.h>
#include <JY901.h>
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>

ros::NodeHandle  nh;

std_msgs::Float32MultiArray imu_msg;
ros::Publisher imu_pub("jy901_data", &imu_msg);


void setup()
{
  Serial.begin(57600);
  Serial1.begin(9600);
  nh.initNode();
  nh.advertise(imu_pub);
}

void loop()
{
  // get acc data
  float accx = JY901.stcAcc.a[0] / 32768 * 16;
  float accy = JY901.stcAcc.a[1] / 32768 * 16;
  float accz = JY901.stcAcc.a[2] / 32768 * 16;
 
  // get gyro data
  float gyrox = JY901.stcGyro.w[0] / 32768 * 2000;
  float gyroy = JY901.stcGyro.w[1] / 32768 * 2000;
  float gyroz = JY901.stcGyro.w[2] / 32768 * 2000;

  // get angle data
  float angler = JY901.stcAngle.Angle[0] / 32768 * 180;
  float anglep = JY901.stcAngle.Angle[1] / 32768 * 180;
  float angley = JY901.stcAngle.Angle[2] / 32768 * 180;

  imu_msg.data_length = 9;
  imu_msg.data[0] = accx;
  imu_msg.data[1] = accy;
  imu_msg.data[2] = accz;
  imu_msg.data[3] = gyrox;
  imu_msg.data[4] = gyroy;
  imu_msg.data[5] = gyroz;
  imu_msg.data[6] = angler;
  imu_msg.data[7] = anglep;
  imu_msg.data[8] = angley;
  
  imu_pub.publish( &imu_msg );
  nh.spinOnce();

  delay(50);

  while (Serial1.available())
  {
    JY901.CopeSerialData(Serial1.read()); //Call JY901 data cope function
  }

}

打开ros

roscore

运行串口（需提前安装rosserial）

rosrun rosserial_python serial_node.py /dev/ttyUSB0

查看消息

rostopic echo jy901_data

消息如下：

经过多次尝试，ros一次发布太多floatarray 会导致后面数据因为存储问题而丢失，所以分三个topic发布
修改代码如下

#include <Wire.h>
#include <JY901.h>
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
ros::NodeHandle  nh;

std_msgs::Float32MultiArray acc_msg;
ros::Publisher acc_pub("jy901_acc", &acc_msg);
std_msgs::Float32MultiArray gyro_msg;
ros::Publisher gyro_pub("jy901_gyro", &gyro_msg);
std_msgs::Float32MultiArray ang_msg;
ros::Publisher ang_pub("jy901_angle", &ang_msg);

void setup()
{
  Serial.begin(57600);
  Serial1.begin(9600);
  nh.initNode();
  nh.advertise(acc_pub);
  nh.advertise(gyro_pub);
  nh.advertise(ang_pub);
}
void loop() 
{
  
  nh.spinOnce();

  acc_msg.data_length = 3;
  acc_msg.data[0] = (float)JY901.stcAcc.Angle[0] / 32768 * 16;
  acc_msg.data[1] = (float)JY901.stcAcc.Angle[1] / 32768 * 16;
  acc_msg.data[2] = (float)JY901.stcAcc.Angle[2] / 32768 * 16;

 acc_pub.publish( &acc_msg );

  delay(100);
  
  gyro_msg.data_length = 3;
  gyro_msg.data[0] = (float)JY901.stcGyro.Angle[0] / 32768 * 2000;
  gyro_msg.data[1] = (float)JY901.stcGyro.Angle[1] / 32768 * 2000;
  gyro_msg.data[2] = (float)JY901.stcGyro.Angle[2] / 32768 * 2000;

  gyro_pub.publish( &gyro_msg );

  delay(100);
  
  ang_msg.data_length = 3;
  ang_msg.data[0] = (float)JY901.stcAngle.Angle[0] / 32768 * 180;
  ang_msg.data[1] = (float)JY901.stcAngle.Angle[1] / 32768 * 180;
  ang_msg.data[2] = (float)JY901.stcAngle.Angle[2] / 32768 * 180;

  ang_pub.publish( &ang_msg );

  delay(100);
   
  while (Serial1.available()) 
  {
    JY901.CopeSerialData(Serial1.read()); //Call JY901 data cope function
  }
}

四、ros中处理数据

订阅topic即可，具体步骤略