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_); } }