diff --git a/README.md b/README.md index 81d7690..3d30abc 100644 --- a/README.md +++ b/README.md @@ -15,3 +15,22 @@ If you require any assistance using this code, please email support@advancednavi Installation, build, device configuration, and execution instructions can be found in the file "Advanced Navigation ROS Driver Notes.txt". + +*** modifications by Kyler Laird *** + +Orientation now complies with REP 103. East is zero degrees. Degrees increment counter-clockwise. +https://github.com/ros-drivers/advanced_navigation_driver/issues/3#issuecomment-372348146 + +new parameters: + utm_zone: Specify a UTM Zone number. This will be used for calculating the transform. + +new topics: + rtcm: Strings published to this topic will be passed to the device as RTCM corrections. + odom: Odometry from the device is published to this topic. (Depends on utm_zone.) + +new frames: + ~: This frame has the name of the current name space and describes the position and orientation of the device. (Depends on utm_zone.) + +Under Linux, serial port reads are blocking. This reduces CPU usage from 100% to 5% (for 20 Hz on an i5). + +These changes are all tentative and need testing. diff --git a/logs/2019-07-08-20-05-56.bag b/logs/2019-07-08-20-05-56.bag new file mode 100644 index 0000000..bf7bbfd Binary files /dev/null and b/logs/2019-07-08-20-05-56.bag differ diff --git a/logs/README.md b/logs/README.md new file mode 100644 index 0000000..544e4bd --- /dev/null +++ b/logs/README.md @@ -0,0 +1,3 @@ +I'll place bag files here from the AN driver. + +I make no guarantees about these files. Please tell me if you notice problems with them. diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 8a574cd..539eaf0 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -26,12 +26,23 @@ * DEALINGS IN THE SOFTWARE. */ +/* +2018.02.09 Kyler Laird +added RTCM handling, configuration parameters + +2018.03.10 Kyler Laird +added REP compliance, transform +*/ + #include +#include +#include #include #include #include #include #include +#include #include "rs232/rs232.h" #include "an_packet_protocol.h" @@ -42,33 +53,154 @@ #include #include #include +#include +#include #define RADIANS_TO_DEGREES (180.0/M_PI) + +int an_packet_transmit(an_packet_t *an_packet) +{ + an_packet_encode(an_packet); + return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet)); +} + +void set_filter_options() +{ + an_packet_t *an_packet; + filter_options_packet_t filter_options_packet; + + /* initialise the structure by setting all the fields to zero */ + memset(&filter_options_packet, 0, sizeof(filter_options_packet_t)); + + filter_options_packet.permanent = TRUE; + filter_options_packet.vehicle_type = vehicle_type_car; + filter_options_packet.internal_gnss_enabled = TRUE; + filter_options_packet.atmospheric_altitude_enabled = TRUE; + filter_options_packet.velocity_heading_enabled = TRUE; + filter_options_packet.reversing_detection_enabled = TRUE; + filter_options_packet.motion_analysis_enabled = TRUE; + + an_packet = encode_filter_options_packet(&filter_options_packet); + + an_packet_transmit(an_packet); + + an_packet_free(&an_packet); +} + +void set_filter_options_x() +{ + //an_packet_t *an_packet = an_packet_allocate(17, 186); + + an_packet_t *an_packet = an_packet_allocate(4, 55); + memcpy(&an_packet->data[0], "test", 5 * sizeof(uint8_t)); + an_packet_transmit(an_packet); + an_packet_free(&an_packet); +} + +void handle_rtcm(const std_msgs::String::ConstPtr& msg) { + const char *rtcm_data; + uint32_t string_length = msg->data.length(); + + // ROS_INFO("RTCM: %d bytes", string_length); + + an_packet_t *an_packet = an_packet_allocate(string_length, packet_id_rtcm_corrections); + memcpy(&an_packet->data[0], &msg->data[0], string_length); + an_packet_transmit(an_packet); + an_packet_free(&an_packet); +} + + +// LatLong-UTM.c++ +// Conversions: LatLong to UTM; and UTM to LatLong; +// by Eugene Reimer, ereimer@shaw.ca, 2002-December; +// with LLtoUTM & UTMtoLL routines based on those by Chuck Gantz chuck.gantz@globalstar.com; +// with ellipsoid & datum constants from Peter H Dana website (http://www.colorado.edu/geography/gcraft/notes/datum/edlist.html); +// +// Usage: see the Usage() routine below; +// +// Copyright © 1995,2002,2010 Eugene Reimer, Peter Dana, Chuck Gantz. Released under the GPL; see http://www.gnu.org/licenses/gpl.html +// (Peter Dana's non-commercial clause precludes using the LGPL) + + +#include //2010-08-11: was +#include //2010-08-11: was + +#define fr 298.257223563 +#define a 6378137 +#define k0 0.9996 +const double PI = 4*atan(1); //Gantz used: PI=3.14159265; +const double deg2rad = PI/180; +const double ee = 2/fr-1/(fr*fr); +const double EE = ee/(1-ee); +double LongOriginRad; + +void LLtoUTM(int Zone, double LatRad, double LongRad, double& Northing, double& Easting) { + // converts LatLong to UTM coords; 3/22/95: by ChuckGantz chuck.gantz@globalstar.com, from USGS Bulletin 1532. + double N, T, C, A, M; + + N = a/sqrt(1-ee*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = EE*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); + + M= a*((1 - ee/4 - 3*ee*ee/64 - 5*ee*ee*ee/256 ) *LatRad + - (3*ee/8 + 3*ee*ee/32 + 45*ee*ee*ee/1024) *sin(2*LatRad) + + (15*ee*ee/256 + 45*ee*ee*ee/1024 ) *sin(4*LatRad) + - (35*ee*ee*ee/3072 ) *sin(6*LatRad)); + + Easting = k0*N*(A+(1-T+C)*A*A*A/6+(5-18*T+T*T+72*C-58*EE)*A*A*A*A*A/120) + 500000.0; + + Northing = k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 + + (61-58*T+T*T+600*C-330*EE)*A*A*A*A*A*A/720)); +} + + + + + + int main(int argc, char *argv[]) { // Set up ROS node // - ros::init(argc, argv, "an_device_node"); - ros::NodeHandle nh; - - if(argc != 3) + ros::init(argc, argv, "an_device"); + ros::NodeHandle nh("~"); + + // Set up the COM port + std::string com_port_s; + nh.param("port", com_port_s, "/dev/ttyS0"); + char *com_port = (char *)com_port_s.c_str(); + + int baud_rate; + nh.param("baud", baud_rate, 115200); + + if (OpenComport(com_port, baud_rate)) { - printf("\nCannot start - not enough commnand line arguments. \nUsage: rosrun an_driver an_driver {port} {baud rate}. \nTry: rosrun an_driver an_driver /dev/ttyUSB0 115200\n"); - printf("Number of command line arguments detected: %i\n",argc); + ROS_INFO("Could not open serial port %s at %d baud.", com_port, baud_rate); exit(EXIT_FAILURE); } + ROS_INFO("port:%s@%d", com_port, baud_rate); + - printf("\nYour Advanced Navigation ROS driver is currently running\nClose the Terminal window when done.\n"); - - // Set up the COM port - char* com_port = argv[1]; - int baud_rate = atoi(argv[2]); + // If a UTM Zone is provided, publish transforms. + // The zone is static to avoid problems due to changing when near a Zone boundary. + int utm_zone; + std::string tf_name = nh.getNamespace(); + tf::Transform transform; + if (nh.getParam("utm_zone", utm_zone)) { + LongOriginRad = (utm_zone*6 - 183) * deg2rad; + ROS_INFO("using UTM Zone %d to publish transform %s", utm_zone, tf_name.c_str()); + + } + - // Initialise Publishers and Topics // - ros::Publisher nav_sat_fix_pub=nh.advertise("an_device/NavSatFix",10); - ros::Publisher twist_pub=nh.advertise("an_device/Twist",10); - ros::Publisher imu_pub=nh.advertise("an_device/Imu",10); - ros::Publisher system_status_pub=nh.advertise("an_device/SystemStatus",10); - ros::Publisher filter_status_pub=nh.advertise("an_device/FilterStatus",10); + // Initialise Publishers, and Subscribers // + ros::Publisher nav_sat_fix_pub=nh.advertise("NavSatFix",10); + ros::Publisher twist_pub=nh.advertise("Twist",10); + ros::Publisher imu_pub=nh.advertise("Imu",10); + ros::Publisher system_status_pub=nh.advertise("SystemStatus",10); + ros::Publisher filter_status_pub=nh.advertise("FilterStatus",10); + ros::Publisher odom_pub = nh.advertise("odom", 50); + ros::Subscriber rtcm_sub = nh.subscribe("rtcm", 1000, handle_rtcm); // Initialise messages sensor_msgs::NavSatFix nav_sat_fix_msg; @@ -126,16 +258,12 @@ int main(int argc, char *argv[]) { quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; int bytes_received; - if (OpenComport(com_port, baud_rate)) - { - printf("Could not open serial port: %s \n",com_port); - exit(EXIT_FAILURE); - } an_decoder_initialise(&an_decoder); + // Loop continuously, polling for packets - while (1) + while (ros::ok()) { if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0) { @@ -145,6 +273,18 @@ int main(int argc, char *argv[]) { // decode all the packets in the buffer // while ((an_packet = an_packet_decode(&an_decoder)) != NULL) { + // acknowledgement packet // + if (an_packet->id == 0) + { + ROS_INFO("acknowledgement data: %d", an_packet->data[3]); + } + + // receiver information packet // + if (an_packet->id == 69) + { + ROS_INFO("receiver information: %d", an_packet->data[0]); + } + // system state packet // if (an_packet->id == packet_id_system_state) { @@ -153,21 +293,21 @@ int main(int argc, char *argv[]) { // NavSatFix nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds; nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000; - if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || - (system_state_packet.filter_status.b.gnss_fix_type == 2)) + if ((system_state_packet.filter_status.b.gnss_fix_type == 1) || // 2D + (system_state_packet.filter_status.b.gnss_fix_type == 2)) // 3D { - nav_sat_fix_msg.status.status=0; + nav_sat_fix_msg.status.status=0; // no fix } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || - (system_state_packet.filter_status.b.gnss_fix_type == 5)) + else if ((system_state_packet.filter_status.b.gnss_fix_type == 3) || // SBAS + (system_state_packet.filter_status.b.gnss_fix_type == 5)) // Omnistar/Starfire { - nav_sat_fix_msg.status.status=1; + nav_sat_fix_msg.status.status=1; // SBAS } - else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || - (system_state_packet.filter_status.b.gnss_fix_type == 6) || - (system_state_packet.filter_status.b.gnss_fix_type == 7)) + else if ((system_state_packet.filter_status.b.gnss_fix_type == 4) || // differential + (system_state_packet.filter_status.b.gnss_fix_type == 6) || // RTK float + (system_state_packet.filter_status.b.gnss_fix_type == 7)) // RTK fixed { - nav_sat_fix_msg.status.status=2; + nav_sat_fix_msg.status.status=2; // GBAS } else { @@ -191,28 +331,92 @@ int main(int argc, char *argv[]) { // IMU imu_msg.header.stamp.sec=system_state_packet.unix_time_seconds; imu_msg.header.stamp.nsec=system_state_packet.microseconds*1000; + // Convert roll, pitch, yaw from radians to quaternion format // - float phi = system_state_packet.orientation[0] / 2.0f; - float theta = system_state_packet.orientation[1] / 2.0f; - float psi = system_state_packet.orientation[2] / 2.0f; - float sin_phi = sinf(phi); - float cos_phi = cosf(phi); - float sin_theta = sinf(theta); - float cos_theta = cosf(theta); - float sin_psi = sinf(psi); - float cos_psi = cosf(psi); - imu_msg.orientation.x=-cos_phi * sin_theta * sin_psi + sin_phi * cos_theta * cos_psi; - imu_msg.orientation.y=cos_phi * sin_theta * cos_psi + sin_phi * cos_theta * sin_psi; - imu_msg.orientation.z=cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi; - imu_msg.orientation.w=cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi; - + tf::Quaternion orientation; + orientation.setRPY( + system_state_packet.orientation[0], + system_state_packet.orientation[1], + PI / 2.0f - system_state_packet.orientation[2] // REP 103 + ); + imu_msg.orientation.x = orientation[0]; + imu_msg.orientation.y = orientation[1]; + imu_msg.orientation.z = orientation[2]; + imu_msg.orientation.w = orientation[3]; + imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1]; imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2]; imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; - + + // transform + if (utm_zone) { + static tf::TransformBroadcaster transform_br; + double N, E; + + LLtoUTM( + utm_zone, + system_state_packet.latitude, + system_state_packet.longitude, + N, E + ); + + transform.setOrigin(tf::Vector3( + E, N, system_state_packet.height + )); + + transform.setRotation(orientation); + + transform_br.sendTransform(tf::StampedTransform( + transform, + ros::Time( + system_state_packet.unix_time_seconds, + system_state_packet.microseconds*1000 + ), + "world", // Is it reasonable to hardcode this? + tf_name + )); + + nav_msgs::Odometry odom; + odom.header.stamp.sec=system_state_packet.unix_time_seconds; + odom.header.stamp.nsec=system_state_packet.microseconds*1000; + + odom.header.frame_id= "world"; // fix this! + odom.child_frame_id = "ins"; // fix this! + + //set the position + odom.pose.pose.position.x = E; + odom.pose.pose.position.y = N; + odom.pose.pose.position.z = system_state_packet.height; + odom.pose.pose.orientation.x = orientation[0]; + odom.pose.pose.orientation.y = orientation[1]; + odom.pose.pose.orientation.z = orientation[2]; + odom.pose.pose.orientation.w = orientation[3]; + + // Is this correct??? + odom.pose.covariance={ + pow(system_state_packet.standard_deviation[1],2), 0.0, 0.0, + 0.0, pow(system_state_packet.standard_deviation[0],2), 0.0, + 0.0, 0.0, pow(system_state_packet.standard_deviation[2],2) + }; + + //set the velocity + odom.twist.twist.linear.x = system_state_packet.velocity[0]; + odom.twist.twist.linear.y = system_state_packet.velocity[1]; + odom.twist.twist.linear.z = system_state_packet.velocity[2]; + + // Set angular velocity. + odom.twist.twist.angular.x = system_state_packet.angular_velocity[0]; + odom.twist.twist.angular.y = system_state_packet.angular_velocity[1]; + odom.twist.twist.angular.z = system_state_packet.angular_velocity[2]; + + //publish the message + odom_pub.publish(odom); + + } + // System Status system_status_msg.message = ""; system_status_msg.level = 0; // default OK state @@ -391,7 +595,7 @@ int main(int argc, char *argv[]) { imu_msg.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2]; } } - // Ensure that you free the an_packet when your done with it // + // Ensure that you free the an_packet when you're done with it // // or you will leak memory // an_packet_free(&an_packet); @@ -403,12 +607,7 @@ int main(int argc, char *argv[]) { filter_status_pub.publish(filter_status_msg); } } + ros::spinOnce(); } } - - - - - - diff --git a/src/rs232/rs232.c b/src/rs232/rs232.c index 2b30f66..b2d3f8d 100644 --- a/src/rs232/rs232.c +++ b/src/rs232/rs232.c @@ -101,7 +101,8 @@ int OpenComport(char *comport, int baudrate) break; } - Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY); + // Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY); + Cport = open(comport, O_RDWR | O_NOCTTY); if(Cport==-1) { perror("unable to open comport "); @@ -122,7 +123,8 @@ int OpenComport(char *comport, int baudrate) new_port_settings.c_oflag = 0; new_port_settings.c_lflag = 0; new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ - new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ + // new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ + new_port_settings.c_cc[VTIME] = 1; /* block untill a timer expires (n * 100 mSec.) */ error = tcsetattr(Cport, TCSANOW, &new_port_settings); if(error==-1) {