Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Cov matrice odometry #184

Open
wants to merge 2 commits into
base: indigo-devel
Choose a base branch
from
Open

Cov matrice odometry #184

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7125ef0
Cov matrice odometry
jbler Apr 8, 2016
3eff6a6
Cov matrice odometry
jbler Apr 8, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion include/ardrone_autonomy/ardrone_driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ class ARDroneDriver
ARDroneDriver();
~ARDroneDriver();
void run();
static bool ReadCovParams(const std::string& param_name, boost::array<double, 9> &cov_array);
static bool ReadCovParams(const std::string& param_name, double *cov_array, const int cov_size);
static double CalcAverage(const std::vector<double> &vec);
void PublishVideo();
void PublishNavdata(const navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time);
Expand Down Expand Up @@ -121,6 +121,8 @@ class ARDroneDriver
bool is_inited;
std::string drone_frame_id;

nav_msgs::Odometry odo_msg;

// Load auto-generated declarations for full navdata
#define NAVDATA_STRUCTS_HEADER_PRIVATE
#include <ardrone_autonomy/NavdataMessageDefinitions.h>
Expand All @@ -135,6 +137,7 @@ class ARDroneDriver

// Huge part of IMU message is constant, let's fill'em once.
sensor_msgs::Imu imu_msg;

geometry_msgs::Vector3Stamped mag_msg;
ardrone_autonomy::Navdata legacynavdata_msg;

Expand Down
19 changes: 16 additions & 3 deletions launch/ardrone.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +26,22 @@
<param name="realtime_navdata" value="true" />
<param name="realtime_video" value="true" />
<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
<rosparam param="cov/imu_la">[1.1, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[2.1, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0]</rosparam>
<rosparam param="cov/imu_or">[1.3, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 100000.0]</rosparam>
<!-- Covariance Values of Odometry (6x6 matrices reshaped to 1x36)-->
<rosparam param="cov/odo_pose">[0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
<rosparam param="cov/odo_twist">[0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01]</rosparam>
</node>
</launch>

25 changes: 17 additions & 8 deletions src/ardrone_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,15 +84,25 @@ ARDroneDriver::ARDroneDriver()
imu_msg.angular_velocity_covariance[i] = 0.0;
imu_msg.orientation_covariance[i] = 0.0;
}
ReadCovParams("~cov/imu_la", imu_msg.linear_acceleration_covariance);
ReadCovParams("~cov/imu_av", imu_msg.angular_velocity_covariance);
ReadCovParams("~cov/imu_or", imu_msg.orientation_covariance);
ReadCovParams("~cov/imu_la", imu_msg.linear_acceleration_covariance.data(), 9);
ReadCovParams("~cov/imu_av", imu_msg.angular_velocity_covariance.data(), 9);
ReadCovParams("~cov/imu_or", imu_msg.orientation_covariance.data(), 9);

if (private_nh.hasParam("do_imu_caliberation"))
{
ROS_WARN("IMU Caliberation has been deprecated since 1.4.");
}

// Fill constant parts of Odometry Message
// If no rosparam is set then the default value of 0.0 will be assigned to all covariance values
for (int i = 0; i < 36; i++)
{
odo_msg.pose.covariance[i] = 0.0;
odo_msg.twist.covariance[i] = 0.0;
}
ReadCovParams("~cov/odo_pose", odo_msg.pose.covariance.data(), 36);
ReadCovParams("~cov/odo_twist", odo_msg.twist.covariance.data(), 36);

// Camera Info Manager
cinfo_hori = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/front"), "ardrone_front");
cinfo_vert = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/bottom"), "ardrone_bottom");
Expand Down Expand Up @@ -229,7 +239,7 @@ double ARDroneDriver::CalcAverage(const std::vector<double> &vec)
return (ret / vec.size());
}

bool ARDroneDriver::ReadCovParams(const std::string &param_name, boost::array<double, 9> &cov_array)
bool ARDroneDriver::ReadCovParams(const std::string &param_name, double* cov_array, const int cov_size)
{
XmlRpc::XmlRpcValue cov_list;
std::stringstream str_stream;
Expand All @@ -241,17 +251,17 @@ bool ARDroneDriver::ReadCovParams(const std::string &param_name, boost::array<do
return false;
}

if (cov_list.size() != 9)
if (cov_list.size() != cov_size)
{
ROS_WARN("Covariance list size for %s is not of size 9 (Size: %d)", param_name.c_str(), cov_list.size());
ROS_WARN("Covariance list size for %s is not of correct size (Size: %d)", param_name.c_str(), cov_list.size());
return false;
}
str_stream << param_name << " set to [";
for (int32_t i = 0; i < cov_list.size(); i++)
{
ROS_ASSERT(cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
cov_array[i] = static_cast<double>(cov_list[i]);
str_stream << cov_array[i] << ((i < 8) ? ", " : "");
str_stream << cov_array[i] << ((i < cov_size - 1) ? ", " : "");
}
str_stream << "]";
ROS_INFO("%s", str_stream.str().c_str());
Expand Down Expand Up @@ -695,7 +705,6 @@ void ARDroneDriver::PublishOdometry(const navdata_unpacked_t &navdata_raw, const
}
last_receive_time = navdata_receive_time;

nav_msgs::Odometry odo_msg;
odo_msg.header.stamp = navdata_receive_time;
odo_msg.header.frame_id = "odom";
odo_msg.child_frame_id = drone_frame_base;
Expand Down