NavdataI::NavdataI()
{
std::cout << "navdata start" << std::endl;
data=new jderobot::NavdataData();
last_navdata_id = -1;
// Fill constant parts of IMU 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 < 9; i++)
{
}
readCovParams();
// Caliberation
max_num_samples = 50;
/*do_caliberation = (ros::param::get("~do_imu_caliberation", do_caliberation)) ? do_caliberation : false;
if (do_caliberation) {
resetCaliberation();
ROS_WARN("Automatic IMU Caliberation is active.");
}*/
}
ARDroneDriver::ARDroneDriver()
: image_transport(node_handle),
// Ugly: This has been defined in the template file. Cleaner way to guarantee initilaztion?
initialized_navdata_publishers(false)
{
inited = false;
last_frame_id = -1;
last_navdata_id = -1;
cmd_vel_sub = node_handle.subscribe("cmd_vel", 1, &cmdVelCallback);
takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
land_sub = node_handle.subscribe("ardrone/land", 1, &landCallback);
image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback);
setFlightAnimation_service = node_handle.advertiseService("ardrone/setflightanimation", setFlightAnimationCallback);
magCalib_service = node_handle.advertiseService("ardrone/magcalib", MagnitoCalibCallback);
usbdata_pub = node_handle.advertise<std_msgs::String>("ardrone/usbdata", 1);//add Alex
ranges_pub = node_handle.advertise<ardrone_autonomy::Ranges>("ardrone/ranges", 1);//add Alex
/*
To be honest, I am not sure why advertising a service using class members should be this complicated!
One day, I should learn what is exactly happenning here. /M
*/
imuReCalib_service = node_handle.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>
("ardrone/imu_recalib", boost::bind(&ARDroneDriver::imuReCalibCallback, this, _1, _2));
// setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
// setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);
droneFrameId = (ros::param::get("~drone_frame_id", droneFrameId)) ? droneFrameId : "ardrone_base";
droneFrameBase = droneFrameId + "_link";
droneFrameIMU = droneFrameId + "_imu";
droneFrameFrontCam = droneFrameId + "_frontcam";
droneFrameBottomCam = droneFrameId + "_bottomcam";
drone_root_frame = (ros::param::get("~root_frame", drone_root_frame)) ? drone_root_frame : ROOT_FRAME_BASE;
drone_root_frame = drone_root_frame % ROOT_FRAME_NUM;
ROS_INFO("Root Frame is: %d", drone_root_frame);
// Fill constant parts of IMU 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 < 9; i++)
{
imu_msg.linear_acceleration_covariance[i] = 0.0;
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);
// Caliberation
max_num_samples = 50;
do_caliberation = (ros::param::get("~do_imu_caliberation", do_caliberation)) ? do_caliberation : false;
if (do_caliberation) {
resetCaliberation();
ROS_WARN("Automatic IMU Caliberation is active.");
}
// 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");
// TF Stuff
// Front Cam to Base
// TODO: Precise values for Drone1 & Drone2
tf_base_front = tf::StampedTransform(
tf::Transform(
tf::createQuaternionFromRPY(-90.0 * _DEG2RAD, 0.0, -90.0 * _DEG2RAD),
tf::Vector3(0.21, 0.0, 0.0)),
ros::Time::now(), droneFrameBase, droneFrameFrontCam
);
// Bottom Cam to Base (Bad Assumption: No translation from IMU and Base)
// TODO: This should be different from Drone 1 & 2.
tf_base_bottom = tf::StampedTransform(
tf::Transform(
tf::createQuaternionFromRPY(180.0 * _DEG2RAD, 0.0, 90.0 * _DEG2RAD),
tf::Vector3(0.0, -0.02, 0.0)),
ros::Time::now(), droneFrameBase, droneFrameBottomCam
);
// Changing the root for TF if needed
if (drone_root_frame == ROOT_FRAME_FRONT)
{
tf_base_front.setData(tf_base_front.inverse());
tf_base_front.child_frame_id_.swap(tf_base_front.frame_id_);
}
else if (drone_root_frame == ROOT_FRAME_BOTTOM)
{
tf_base_bottom.setData(tf_base_bottom.inverse());
tf_base_bottom.child_frame_id_.swap(tf_base_bottom.frame_id_);
}
}
