I am using rosserial to transmit gyroscope values into ROS. I am initializing the message using the format I found [here](http://answers.ros.org/question/12782/rosserial_arduino-cant-send-a-sensor_msgsimu-msg/). At first I got an error saying the buffer was too small, so I changed the ATMEGA328P field in ros.h from "25, 25, 280, 280" to "5, 5, 512, 512".
This worked, but then I started getting errors like:
[INFO] [WallTime: 1428333648.941214] wrong checksum for topic id and msg
[WARN] [WallTime: 1428333651.605690] Serial Port read returned short (expected 1 bytes, received 0 instead).
[WARN] [WallTime: 1428333651.608523] Serial Port read failure:
[WARN] [WallTime: 1428333651.626828] Serial Port read returned short (expected 8 bytes, received 1 instead).
[WARN] [WallTime: 1428333651.629609] Serial Port read failure:
[INFO] [WallTime: 1428333651.633695] Packet Failed : Failed to read msg data
[INFO] [WallTime: 1428333651.636200] msg len is 8
[INFO] [WallTime: 1428333651.646104] wrong checksum for topic id and msg
[INFO] [WallTime: 1428333653.732366] wrong checksum for topic id and msg
[WARN] [WallTime: 1428333657.162546] Serial Port read returned short (expected 8 bytes, received 5 instead).
From google searches, the only possible explanation I could find was to change the baud rate, however I must not have implemented it correctly as it this resulted in the error (yes, its supposed to be ttyUSB1):
[INFO] [WallTime: 1428335580.534810] Connecting to /dev/ttyUSB1 at 115200 baud
[ERROR] [WallTime: 1428335598.989444] Lost sync with device, restarting...
I am unsure of what to try next. Any suggestions are appreciated.
Here is my code:
#define pi 3.14159265359
#include
#include
ros::NodeHandle nh;
sensor_msgs::Imu imu_msg;
ros::Publisher gyro("/imu/data", &imu_msg);
int gyroPin = 0; //Gyro is connected to analog pin 0
float gyroVoltage = 5; //Gyro is running at 5V
float gyroZeroVoltage = 2.5; //Gyro is zeroed at 2.5V
float gyroSensitivity = .0125; //Our example gyro is 12.5mV/deg/sec
float rotationThreshold = 1; //Minimum deg/sec to keep track of - helps with gyro drifting
float currentAngle = 0; //Keep track of our current angle
void setup()
{
//nh.getHardware()->setBaud(115200); //or what ever baud you want
nh.initNode();
nh.advertise(gyro);
imu_msg.header.frame_id = 0;
imu_msg.orientation.x = 0.0;
imu_msg.orientation.y = 0.0;
imu_msg.orientation.z = 0.0;
imu_msg.orientation.w = 0.0;
}
void loop()
{
//This line converts the 0-1023 signal to 0-5V
float gyroRate = (analogRead(gyroPin) * gyroVoltage) / 1023;
//This line finds the voltage offset from sitting still
gyroRate -= gyroZeroVoltage;
//This line divides the voltage we found by the gyro's sensitivity
gyroRate /= gyroSensitivity;
//Ignore the gyro if our angular velocity does not meet our threshold
if (gyroRate >= rotationThreshold || gyroRate <= -rotationThreshold) {
//This line divides the value by 100 since we are running in a 10ms loop (1000ms/10ms)
gyroRate /= 100;
currentAngle += gyroRate;
}
//Keep our angle between 0-359 degrees
if (currentAngle < 0)
currentAngle += 360;
else if (currentAngle > 359)
currentAngle -= 360;
currentAngle = currentAngle * (pi/180); //convert degrees/s to radians/s
imu_msg.orientation.z = currentAngle;
imu_msg.header.stamp = nh.now();
gyro.publish( &imu_msg );
nh.spinOnce();
delay(1000);
}
↧