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
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]
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()
Code: Select all
cout
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;
}
}
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
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
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