int main(int argc, char **argv) { int x,y,z; int minx,miny,maxy,maxx; union { heading_evt_t hd; magnetometer_evt_t magn; evt_head_t head; char buf[256]; } evt; init_compass_calib(); minx=miny=32767; maxx=maxy=-32768; while(fread(&evt,1,sizeof(evt.head),stdin)) { fread(evt.buf+sizeof(evt.head),1,evt.head.len-sizeof(evt.head),stdin); switch(evt.head.type) { case EVENT_MAGNETOMETER: case EVENT_MAGNETOMETER_EXT: calculate_heading(&evt.magn,&x,&y,&z); printf("x: %d y: %d z: %d\n",x,y,z); if (x<minx) minx=x; if (x>maxx) maxx=x; if (y<miny) miny=y; if (y>maxy) maxy=y; break; } } fprintf(stderr,"MinX: %d MaxX: %d, pp: %d\n",minx,maxx,maxx-minx); fprintf(stderr,"MinY: %d MaxY: %d, pp: %d\n",miny,maxy,maxy-miny); printf("%d %d 0\n",-((maxx-minx)/2+minx),-((maxy-miny)/2+miny)); return 0; }
void utmCallback(const msgs::gpgga_utm::ConstPtr& msg) { static tf::TransformBroadcaster odom_broadcaster; if(msg->hdop < 2 && utm_settled == false) { utm_settled_count++; if(utm_settled_count >= utm_settled_count_top) { utm_settled = true; ref_fix = *msg; prev_msg = *msg; } } else if(utm_settled) { gps_points_t p; p.x = msg->utm_e; p.y = msg->utm_n; gps_points_buffer.push_front(p); odom.header.stamp = ros::Time::now(); odom.header.frame_id =frame_id; odom.child_frame_id = child_frame_id; if(publish_relative){ odom.pose.pose.position.x = -(ref_fix.utm_e - msg->utm_e); odom.pose.pose.position.y = -(ref_fix.utm_n - msg->utm_n); } else { // ENU odom.pose.pose.position.x =msg->utm_e; odom.pose.pose.position.y =msg->utm_n; } odom.pose.pose.position.z = 0; static double yaw=0; if(calculate_heading(yaw)) { odom.pose.covariance[35] = heading_variance; } else { odom.pose.covariance[35] = 999999; } // publish the transform message odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = tf_frame_id; odom_trans.child_frame_id = tf_child_frame_id; odom_trans.transform.translation.x = odom.pose.pose.position.x; odom_trans.transform.translation.y = odom.pose.pose.position.y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(yaw); odom_broadcaster.sendTransform(odom_trans); odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); odom.pose.covariance[0] = msg->hdop * gps_variance; odom.pose.covariance[7] = msg->hdop * gps_variance; odom.pose.covariance[14] = 999999; odom.pose.covariance[21] = 999999; odom.pose.covariance[28] = 999999; odom_pub.publish(odom); prev_msg = *msg; } }