Beispiel #1
0
	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_);
    }

}