komodo_sensors.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "komodo_sensors/apm_adc.h"
#include "komodo_sensors/apm_gps.h"
#include "komodo_sensors/apm_imu.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/Range.h"
#include "sensor_msgs/NavSatFix.h"
#include "sensor_msgs/NavSatStatus.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Vector3Stamped.h"
#include "tf/transform_datatypes.h"
#include "std_msgs/Float32.h"
//#include <tf/transform_broadcaster.h>

ros::Publisher pc_bat_pub;

ros::Publisher imu_raw_pub;
ros::Publisher imu_mag_pub;

ros::Publisher us1_pub;
ros::Publisher us2_pub;
ros::Publisher us3_pub;
ros::Publisher ir1_pub;
ros::Publisher ir2_pub;
ros::Publisher ir3_pub;

ros::Publisher gps_pub;

sensor_msgs::Range us1_range_msg;
sensor_msgs::Range us2_range_msg;
sensor_msgs::Range us3_range_msg;
sensor_msgs::Range ir1_range_msg;
sensor_msgs::Range ir2_range_msg;
sensor_msgs::Range ir3_range_msg;

std::string gps_frame_id;

//tf::TransformBroadcaster *imu_broadcaster;

void gpsCallback(const komodo_sensors::apm_gps::ConstPtr& msg) {

        // ROS_INFO("lat:%.7f  lon:%.7f  alt:%.2f  satelites:%d  hdop:%d  status:%d",(float)msg->lat/10000000.0,(float)msg->lon/10000000.0,(float)msg->alt/100.0,msg->sats,msg->hdop,msg->status);
        sensor_msgs::NavSatFix gps_msg;
        gps_msg.header.frame_id = gps_frame_id;
        gps_msg.header.stamp = ros::Time::now();
        gps_msg.latitude = (float)msg->lat / 10000000.0;
        gps_msg.longitude = (float)msg->lon / 10000000.0;
        gps_msg.altitude = (float)msg->alt / 100;

        sensor_msgs::NavSatStatus gps_status_msg;

        gps_status_msg.service = 1; //SERVICE_GPS;
        if ((msg->status == 0) || (msg->status == 1))
                gps_status_msg.status = 1; //STATUS_NO_FIX;
        else
                gps_status_msg.status = 0; //STATUS_FIX;

        gps_msg.status = gps_status_msg;

        gps_msg.position_covariance_type = 1;
//uint8 COVARIANCE_TYPE_UNKNOWN = 0
//uint8 COVARIANCE_TYPE_APPROXIMATED = 1
//uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
//uint8 COVARIANCE_TYPE_KNOWN = 3
//float64[9] position_covariance;
        float cov = ((float) (msg->hdop) / 100.0) * ((float) (msg->hdop) / 100.0);
        gps_msg.position_covariance[0] = cov;
        gps_msg.position_covariance[4] = cov;
        gps_msg.position_covariance[8] = cov;
//lat_lon_cov=((float)hdop/100)^2;

//gps_msg.position_covariance=position_covariance;

        gps_pub.publish(gps_msg);
//gps status:
        //NO_GPS = 0,             ///< No GPS connected/detected
        //  NO_FIX = 1,             ///< Receiving valid GPS messages but no lock
        //  GPS_OK = 2              ///< Receiving valid messages and locked
}

void adcCallback(const komodo_sensors::apm_adc::ConstPtr& msg) {
        std_msgs::Float32 v_bat;
        v_bat.data = (float)(msg->A7) * 5.0 / 1023.0;
        pc_bat_pub.publish(v_bat);

        //ROS_INFO("A0:%.2f  A1:%.2f  A2:%.2f  A3:%.2f  A4:%.2f  A5:%.2f  A6:%.2f  A7:%.2f", (float)msg->A0/10,(float)msg->A1/10,(float)msg->A2/10,(float)msg->A3/10,(float)msg->A4/10,(float)msg->A5/10,(float)msg->A6/10,(float)msg->A7/10);
        ros::Time now = ros::Time::now();

        us1_range_msg.header.stamp = now;
        us1_range_msg.range = (float) msg->A0 / 1.7 * 2.54 / 100.0;

        us2_range_msg.header.stamp = ros::Time::now();
        us2_range_msg.range = (float) msg->A1 / 1.7 * 2.54 / 100.0;

        us3_range_msg.header.stamp = now;
        us3_range_msg.range = (float) msg->A2 / 1.7 * 2.54 / 100.0;
        
        ir1_range_msg.header.stamp = now;
        //ir1_range_msg.range = ((6762 / ((float) msg->A3 - 9)) - 6)/100; //from sensor back
        
        
        if (((float)(msg->A3) >=80)&&((float)(msg->A3) <= 500)) ir1_range_msg.range = (4800.0/((float)(msg->A3 - 20)))/100.0; //from sensor back
        else if ((float)(msg->A3) <80) ir1_range_msg.range =ir1_range_msg.max_range;
        else ir1_range_msg.range =ir1_range_msg.min_range;

        
        
        ir2_range_msg.header.stamp = now;
        //ir2_range_msg.range = ((6762 / ((float) msg->A4 - 9)) - 6)/100; //from sensor back
        if (((float)(msg->A4) >=80)&&((float)(msg->A4) <= 500)) ir2_range_msg.range = (4800.0/((float)(msg->A4 - 20)))/100.0; //from sensor back
                else if ((float)(msg->A4) <80) ir2_range_msg.range =ir2_range_msg.max_range;
                else ir2_range_msg.range =ir2_range_msg.min_range;

        
        
        ir3_range_msg.header.stamp = now;
        //ir3_range_msg.range = ((6762 / ((float) msg->A5 - 9)) - 6)/100; //from sensor back
        if (((float)(msg->A5) >=80)&&((float)(msg->A5) <= 500)) ir3_range_msg.range = (4800.0/((float)(msg->A5 - 20)))/100.0; //from sensor back
                else if ((float)(msg->A5) <80) ir3_range_msg.range =ir3_range_msg.max_range;
                else ir3_range_msg.range =ir3_range_msg.min_range;

        
        us1_pub.publish(us1_range_msg);
        us2_pub.publish(us2_range_msg);
        us3_pub.publish(us3_range_msg);
        ir1_pub.publish(ir1_range_msg);
        ir2_pub.publish(ir2_range_msg);
        ir3_pub.publish(ir3_range_msg);
        //ROS_INFO("r0=%2.2f",us1_range_msg.range);
}

void imuCallback(const komodo_sensors::apm_imu::ConstPtr& msg) {

        // ROS_INFO("ax:%.5f  ay:%.5f az:%.5f    gx:%.5f  gy:%.5f gz:%.5f    mx:%.5f  my:%.5f mz:%.5f", msg->ax,msg->ay,msg->az, msg->gx,msg->gy,msg->gz, msg->mx,msg->my,msg->mz);
        //  ROS_INFO("r:%2.2f  p:%2.2f  y:%2.2f    h:%3.2f  ax:%2.3f  ay:%2.3f az:%2.3f    length: %2.2f", msg->roll*57.29577951,msg->pitch*57.29577951,msg->yaw*57.29577951,msg->heading*57.29577951, msg->ax,msg->ay,msg->az, sqrt(msg->ax*msg->ax+msg->ay*msg->ay+msg->az*msg->az ));
        //ROS_INFO("ax:%2.3f  ay:%2.3f az:%2.3f    length: %2.2f", msg->ax,msg->ay,msg->az, sqrt(msg->ax*msg->ax+msg->ay*msg->ay+msg->az*msg->az ));

        sensor_msgs::Imu imu_msg;
        geometry_msgs::Vector3Stamped m;

        imu_msg.header.frame_id = "imu_base_link";
        imu_msg.header.stamp = ros::Time::now();

        imu_msg.linear_acceleration.x = msg->ax;
        imu_msg.linear_acceleration.y = msg->ay;
        imu_msg.linear_acceleration.z = msg->az;
        imu_msg.angular_velocity.x = msg->gx;
        imu_msg.angular_velocity.y = msg->gy;
        imu_msg.angular_velocity.z = msg->gz;

        double angular_velocity_stdev_ = 0.012;
        double orientation_stdev_ = 0.035;
        double linear_acceleration_stdev_ = 0.098;
        double angular_velocity_covariance = angular_velocity_stdev_
                        * angular_velocity_stdev_;
        double orientation_covariance = orientation_stdev_ * orientation_stdev_;
        double linear_acceleration_covariance = linear_acceleration_stdev_
                        * linear_acceleration_stdev_;

        imu_msg.linear_acceleration_covariance[0] = linear_acceleration_covariance;
        imu_msg.linear_acceleration_covariance[4] = linear_acceleration_covariance;
        imu_msg.linear_acceleration_covariance[8] = linear_acceleration_covariance;

        imu_msg.angular_velocity_covariance[0] = angular_velocity_covariance;
        imu_msg.angular_velocity_covariance[4] = angular_velocity_covariance;
        imu_msg.angular_velocity_covariance[8] = angular_velocity_covariance;

        imu_msg.orientation_covariance[0] = orientation_covariance;
        imu_msg.orientation_covariance[4] = orientation_covariance;
        imu_msg.orientation_covariance[8] = orientation_covariance;

        m.header.frame_id = "imu_base_link";
        m.header.stamp = ros::Time::now();
        m.vector.x = msg->mx;
        m.vector.y = msg->my;
        m.vector.z = msg->mz;
        //tf::TransformBroadcaster odom_broadcaster;
        //geometry_msgs::Quaternion Q=tf::createQuaternionMsgFromRollPitchYaw(roll*0.01745329, pitch*0.01745329, yaw*0.01745329);

        //geometry_msgs::Quaternion Q = tf::createQuaternionMsgFromYaw(yaw*0.01745329);
        //imu_msg.orientation=Q;

        imu_raw_pub.publish(imu_msg);
        imu_mag_pub.publish(m);
        /*
         geometry_msgs::TransformStamped imu_trans;
         imu_trans.header.stamp = now;
         imu_trans.header.frame_id = odom_frame_id;
         imu_trans.child_frame_id = "base_footprint";
         odom_trans.transform.rotation = quat;
         imu_broadcaster->sendTransform(imu_trans);*/

}

int main(int argc, char **argv) {

        // ros::init(argc, argv, "apm_talker");
        ros::init(argc, argv, "apm_driver");

        ros::NodeHandle n;

        n.param("GPS_frame_id", gps_frame_id, std::string("/map"));

        ros::Subscriber adc_sub = n.subscribe("apm_adc", 10, adcCallback);
        ros::Subscriber imu_sub = n.subscribe("apm_imu", 10, imuCallback);
        ros::Subscriber gps_sub = n.subscribe("apm_gps", 10, gpsCallback);

        imu_raw_pub = n.advertise < sensor_msgs::Imu > ("imu/data_raw", 10);
        imu_mag_pub = n.advertise < geometry_msgs::Vector3Stamped > ("imu/mag", 10);

        us1_range_msg.header.frame_id = "/Right_US_link";
        us1_range_msg.field_of_view = 0.8;
        us1_range_msg.radiation_type = 0; //us
        us1_range_msg.min_range = 0.2;
        us1_range_msg.max_range = 6.5;

        us2_range_msg.header.frame_id = "/Back_US_link";
        us2_range_msg.field_of_view = 0.8;
        us2_range_msg.radiation_type = 0; //us
        us2_range_msg.min_range = 0.2;
        us2_range_msg.max_range = 6.5;

        us3_range_msg.header.frame_id = "/Left_US_link";
        us3_range_msg.field_of_view = 0.8;
        us3_range_msg.radiation_type = 0; //us
        us3_range_msg.min_range = 0.2;
        us3_range_msg.max_range = 6.5;

        ir1_range_msg.header.frame_id = "/Right_IR_link";
        ir1_range_msg.field_of_view = 0.05;
        ir1_range_msg.radiation_type = 1; //ir
        ir1_range_msg.min_range = 0.1;
        ir1_range_msg.max_range = 0.8;

        ir2_range_msg.header.frame_id = "/Back_IR_link";
        ir2_range_msg.field_of_view = 0.05;
        ir2_range_msg.radiation_type = 1; //ir
        ir2_range_msg.min_range = 0.1;
        ir2_range_msg.max_range = 0.8;

        ir3_range_msg.header.frame_id = "/Left_IR_link";
        ir3_range_msg.field_of_view = 0.05;
        ir3_range_msg.radiation_type = 1; //ir
        ir3_range_msg.min_range = 0.1;
        ir3_range_msg.max_range = 0.8;

        us1_pub = n.advertise < sensor_msgs::Range > ("Rangers/Right_US", 5);
        us2_pub = n.advertise < sensor_msgs::Range > ("Rangers/Back_US", 5);
        us3_pub = n.advertise < sensor_msgs::Range > ("Rangers/Left_US", 5);
        ir1_pub = n.advertise < sensor_msgs::Range > ("Rangers/Right_IR", 5);
        ir2_pub = n.advertise < sensor_msgs::Range > ("Rangers/Back_IR", 5);
        ir3_pub = n.advertise < sensor_msgs::Range > ("Rangers/Left_IR", 5);

        gps_pub = n.advertise < sensor_msgs::NavSatFix > ("gps_publish", 5);

        pc_bat_pub = n.advertise < std_msgs::Float32 > ("pc_bat", 5);

        //imu_broadcaster = new tf::TransformBroadcaster;

        ros::spin();

        return 0;
}