Autonomous Navigation Using ROS

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
jseanm1
Beginner
Posts: 3
Joined: Mon Oct 09, 2017 4:02 am

Autonomous Navigation Using ROS

Post by jseanm1 »

Hi,

I'm trying to use a CrazyFlie for 3D reconstruction.

I followed the instructions for ROS at https://wiki.bitcraze.io/doc:lps:ros to try and hover autonomously. However the ROS node lps_ekf_bridge fails to execute properly.
reques
More specifically, when I try to launch

Code: Select all

roslaunch bitcraze_lps_estimator dwm_loc_ekf_hover.launch uri:=radio://0/80/250K x:=2 y:=3.4 z:=2.5
a runtime error is thrown during the setting up of anchor positions. And confusingly, the runtime error differs from time to time and is thrown during different anchor setups. I have attached the screenshots for two attempts back-to-back without any changes to the system.
22375446_10155160810593915_1354577850_o.png
22396641_10155160810588915_940714424_o.png
My anchor_pos.yaml file looks like this

Code: Select all

n_anchors: 6
anchor0_pos: [ 0.8, 0.0, 2.75]
anchor1_pos: [ 0.0, 3.2, 0.0]
anchor2_pos: [ 0.6, 4.8, 2.75]
anchor3_pos: [ 4.1, 4.8, 0.0]
anchor4_pos: [ 4.1, 3.2, 2.75]
anchor5_pos: [ 4.1, 0.7, 0.0]
Can someone please point out what I should do to fix this?

I'm running on ROS-kinetic, Ubuntun 16.04, and the current master branches of https://github.com/bitcraze/lps-ros and https://github.com/whoenig/crazyflie_ros .

EDIT 1:
In an attempt to debug this, I edited the

Code: Select all

void Crazyflie::requestLogToc()
method and added a

Code: Select all

cout
like this.

Code: Select all

void Crazyflie::requestLogToc()
{
  // Find the number of log variables in TOC
  crtpLogGetInfoRequest infoRequest;
  startBatchRequest();
  addRequest(infoRequest, 1);
  handleRequests();
  size_t len = getRequestResult<crtpLogGetInfoResponse>(0)->log_len;
  std::cout << "Log: " << len << std::endl;

  // Request detailed information
  startBatchRequest();
  for (size_t i = 0; i < len; ++i) {
    crtpLogGetItemRequest itemRequest(i);
    addRequest(itemRequest, 2);
  }
  handleRequests();

  // Update internal structure with obtained data
  m_logTocEntries.resize(len);
  for (size_t i = 0; i < len; ++i) {
    auto response = getRequestResult<crtpLogGetItemResponse>(i);
    LogTocEntry& entry = m_logTocEntries[i];
    entry.id = i;
    entry.type = (LogType)response->type;
    entry.group = std::string(&response->text[0]);
    entry.name = std::string(&response->text[entry.group.size() + 1]);
    std::cout << entry.group << "-" << entry.name << std::endl;
  }
}
The output I got was this.

Code: Select all

-
pwm-m2_pwm
pwm-m3_pwm
pwm-m4_pwm
sys-canfly
-
ext_pos-Y
ext_pos-Z
pid_attitude-roll_outP
pid_attitude-roll_outI
pid_attitude-roll_outD
pid_attitude-pitch_outP
pid_attitude-pitch_outI
pid_attitude-pitch_outD
pid_attitude-yaw_outP
-
pid_attitude-yaw_outD
pid_rate-roll_outP
pid_rate-roll_outI
-
-
-
-
pid_rate-yaw_outP
pid_rate-yaw_outI
pid_rate-yaw_outD
sensorfusion6-qw
-
sensorfusion6-qy
sensorfusion6-qz
sensorfusion6-gravityX
sensorfusion6-gravityY
sensorfusion6-gravityZ
sensorfusion6-accZbase
sensorfusion6-isInit
sensorfusion6-isCalibrated
ctrltarget-roll
ctrltarget-pitch
-
acc-x
acc-y
-
stateEstimate-x
stateEstimate-y
stateEstimate-z
controller-ctr_yaw
mag-x
mag-y
mag-z
baro-asl
baro-temp
-
stabilizer-roll
stabilizer-pitch
stabilizer-yaw
stabilizer-thrust
gyro-x
-
gyro-z
posEstimatorAlt-estimatedZ
posEstimatorAlt-estVZ
posEstimatorAlt-velocityZ
posCtl-targetVX
-
-
posCtl-targetX
posCtl-targetY
-
-
-
posCtl-Xd
posCtl-Yp
posCtl-Yi
posCtl-Yd
-
posCtl-Zi
posCtl-Zd
posCtl-VXp
-
posCtl-VXd
posCtl-VZp
-
posCtl-VZd
controller-actuatorThrust
controller-roll
controller-pitch
controller-yaw
controller-rollRate
-
controller-yawRate
motor-m4
motor-m1
motor-m2
motor-m3
pm-vbat
pm-vbatMV
pm-extVbat
pm-extVbatMV
pm-extCurr
pm-chargeCurrent
pm-state
radio-rssi
extrx-ch0
extrx-ch1
extrx-ch2
extrx-ch3
extrx-thrust
extrx-roll
-
extrx-yaw
kalman_states-ox
kalman_states-oy
kalman_states-vx
kalman_states-vy
kalman_pred-predNX
kalman_pred-predNY
kalman_pred-measNX
kalman_pred-measNY
-
-
-
kalman-stateZ
kalman-statePX
kalman-statePY
kalman-statePZ
kalman-stateD0
kalman-stateD1
kalman-stateD2
kalman-stateSkew
-
kalman-varY
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
And the error message is

Code: Select all

terminate called after throwing an instance of 'std::runtime_error'
  what():  Could not find acc.z in log toc!
Traceback (most recent call last):
  File "/home/janindu/phd_workspace/src/lps-ros/scripts/lps_ekf_bridge.py", line 77, in <module>
    update_params([name + 'x', name + 'y', name + 'z'])
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.service.ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details
Looking at this, it seems that acc.z is missing in the entries. Does it look like transmission corruption?
jseanm1
Beginner
Posts: 3
Joined: Mon Oct 09, 2017 4:02 am

Re: Autonomous Navigation Using ROS

Post by jseanm1 »

We finally sorted this out with a code change.

In crazyflie_cpp/src/crazyclie.cpp, changed the requestLogToc method by replacing

Code: Select all

  // Request detailed information
  startBatchRequest();
  for (size_t i = 0; i < len; ++i) {
    crtpLogGetItemRequest itemRequest(i);
    addRequest(itemRequest, 2);
  }
  handleRequests();

  // Update internal structure with obtained data
  m_logTocEntries.resize(len);
  for (size_t i = 0; i < len; ++i) {
    auto response = getRequestResult<crtpLogGetItemResponse>(i);
    LogTocEntry& entry = m_logTocEntries[i];
    entry.id = i;
    entry.type = (LogType)response->type;
    entry.group = std::string(&response->text[0]);
    entry.name = std::string(&response->text[entry.group.size() + 1]);
    std::cout << entry.group << "-" << entry.name << std::endl;
  }
with

Code: Select all

#if 0
  // Request detailed information
  startBatchRequest();
  for (size_t i = 0; i < len; ++i) {
    crtpLogGetItemRequest itemRequest(i);
    addRequest(itemRequest, 2);
  }
  handleRequests();

  // Update internal structure with obtained data
  m_logTocEntries.resize(len);
  for (size_t i = 0; i < len; ++i) {
    auto response = getRequestResult<crtpLogGetItemResponse>(i);
    LogTocEntry& entry = m_logTocEntries[i];
    entry.id = i;
    entry.type = (LogType)response->type;
    entry.group = std::string(&response->text[0]);
    entry.name = std::string(&response->text[entry.group.size() + 1]);
    std::cout << entry.group << "-" << entry.name << std::endl;
  }
#else
  m_logTocEntries.resize(len);
  for (size_t i = 0; i < len; ++i) {
    startBatchRequest();
    crtpLogGetItemRequest itemRequest(i);
    addRequest(itemRequest, 2);
    handleRequests();
    auto response = getRequestResult<crtpLogGetItemResponse>(0);
    LogTocEntry& entry = m_logTocEntries[i];
    entry.id = i;
    entry.type = (LogType)response->type;
    entry.group = std::string(&response->text[0]);
    entry.name = std::string(&response->text[entry.group.size() + 1]);
    std::cout << entry.group << "-" << entry.name << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
#endif
Essentially, instead of batch requests, each request was processed separately.

Results : https://twitter.com/JaninduAruk/status/ ... 8374378498
Post Reply