[ERROR] Lost sync with device, restarting...
Hi,
Im wroking on a Robot that do the mapping, I'm using UltraSonic Sensors, IMU and Hall Sensors From the BLDC Motor, Arduino Mega 2560 and ROS Kinetic.
When I use only the program of UltraSonic Sensors, it's working fine, and the same thing when I use just the IMU program. but when I try to combine the codes, I get this error :
~$ rosrun rosserial_python serial_node.py /dev/ttyACM0
[INFO] [1509706698.273934]: ROS Serial Python Node
[INFO] [1509706698.281927]: Connecting to /dev/ttyACM0 at 57600 baud
[INFO] [1509706701.073337]: Note: publish buffer size is 512 bytes
[INFO] [1509706701.074074]: Setup publisher on /ultrasound1 [sensor_msgs/Range]
[INFO] [1509706701.078273]: Setup publisher on /ultrasound2 [sensor_msgs/Range]
[INFO] [1509706701.087721]: Setup publisher on /ultrasound3 [sensor_msgs/Range]
[INFO] [1509706701.227182]: Setup publisher on /tf [tf/tfMessage]
[INFO] [1509706701.231467]: Setup publisher on imu_orientation [geometry_msgs/Quaternion]
[INFO] [1509706701.235437]: Setup publisher on imu_gyro [geometry_msgs/Vector3]
[INFO] [1509706701.239664]: Setup publisher on imu_accl [geometry_msgs/Vector3]
[INFO] [1509706701.244122]: Setup publisher on /wheel_velocity [geometry_msgs/Twist]
[WARN] [1509706706.796352]: Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.
[WARN] [1509706745.019652]: Serial Port read returned short (expected 45 bytes, received 30 instead).
[WARN] [1509706745.020506]: Serial Port read failure:
[INFO] [1509706745.021367]: Packet Failed : Failed to read msg data
[INFO] [1509706745.022326]: msg len is 45
[ERROR] [1509706752.960119]: Lost sync with device, restarting...
[WARN] [1509706763.568734]: Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.
^C[INFO] [1509706766.580611]: Send tx stop request
For doing the mapping, you should give to the gmapping the sensor_msgs/LaserScan , so we convert the 3 UltraSound Range to a LaserScan. This is my setup and loop functions Arduino code :
void setup() {
nh.initNode();
nh.advertise(pub_range1);
nh.advertise(pub_range2);
nh.advertise(pub_range3);
range_msg1.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg1.header.frame_id = "/ultrasound1";
range_msg1.field_of_view = 1.570796;
range_msg1.min_range = 0.02;
range_msg1.max_range = 4.0;
range_msg2.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg2.header.frame_id = "/ultrasound2";
range_msg2.field_of_view = 1.570796;
range_msg2.min_range = 0.02;
range_msg2.max_range = 4.0;
range_msg3.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg3.header.frame_id = "/ultrasound3";
range_msg3.field_of_view = 1.570796;
range_msg3.min_range = 0.02;
range_msg3.max_range = 4.0;
for(i=0;i<3;i++){
pinMode(T[i], OUTPUT);
digitalWrite(T[i], LOW);
pinMode(E[i], INPUT);
}
Wire.begin();
nh.initNode();
broadcaster.init(nh);
nh.advertise(imu_ori);
nh.advertise(imu_gyr);
nh.advertise(imu_acc);
mpu.initialize();
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
mpu.setDMPEnabled(true);
attachInterrupt(4, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
pinMode(L_h1, INPUT);
pinMode(L_h2, INPUT);
pinMode(R_h1, INPUT);
pinMode(R_h2, INPUT);
last_left_val_a = digitalRead(L_h1);
last_left_val_b = digitalRead(L_h2);
last_right_val_a = digitalRead(R_h1);
last_right_val_b = digitalRead(R_h2);
Timer1.initialize(LOOP_TIME);
attachInterrupt(0, leftCount, CHANGE);
attachInterrupt(1, leftCount, CHANGE);
attachInterrupt(5, rightCount, CHANGE);
//nh.subscribe(subCmdVel);
nh.advertise(wheel_vel_pub);
Timer1.attachInterrupt(timerIsr);
}
unsigned long range_time;
void loop()
{
if ( millis() >= range_time ){
range_msg1.range = getRange(T[0],E[0]);
range_msg1.header.stamp = nh.now();
pub_range1.publish(&range_msg1);
range_msg2.range = getRange(T[1],E[1]);
range_msg2.header.stamp = nh.now();
pub_range2.publish(&range_msg2);
range_msg3.range = getRange(T[2],E[2]);
range_msg3.header.stamp = nh.now();
pub_range3.publish(&range_msg3);
range_time = millis() + 50;
}
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
}
else if (mpuIntStatus & 0x01)
{
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
orientation.x = q.x;
orientation.y = q.y;
orientation.z = q.z;
orientation.w = q.w;
imu_ori.publish(&orientation);
angular_velocity.x = ypr[0];
angular_velocity.y = ypr[1];
angular_velocity.z = ypr[2];
imu_gyr.publish(&angular_velocity);
linear_acceleration.x = aaReal.x * 1/16384. * 9.80665;
linear_acceleration.y = aaReal.y * 1/16384. * 9.80665;
linear_acceleration.z = aaReal.z * 1/16384. * 9.80665;
imu_acc.publish(&linear_acceleration);
t.header.frame_id = frameid;
t.child_frame_id = child;
t.transform.translation.x = 0.5;
t.transform.rotation.x = q.x;
t.transform.rotation.y = q.y;
t.transform.rotation.z = q.z;
t.transform.rotation.w = q.w;
t.header.stamp = nh.now();
broadcaster.sendTransform(t);
//nh.spinOnce();
//delay(200);
}
nh.spinOnce();
}
Asked by أسامة الادريسي on 2017-11-03 06:30:30 UTC
Comments