diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index 2f163b91..c7b3aed4 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -53,6 +53,14 @@ std::shared_ptr init_client(const std::string& hostname, lidar_mode mode = MODE_UNSPEC, timestamp_mode ts_mode = TIME_FROM_UNSPEC, int lidar_port = 0, int imu_port = 0, + multipurpose_io_mode io_mode = mio_mode_OFF, + nmea_baud_rate baud = BAUD_9600, + nmea_ignore_valid_char nmea_ignore = nmea_val_char_ignore, + nmea_in_polarity nmea_polarity = nmea_polarity_ACTIVE_LOW, + int nmea_leap_seconds = 0, + sync_pulse_in_polarity sync_pulse_pol = sync_pulse_in_ACTIVE_HIGH, + int azimuth_window_start = 0, + int azimuth_window_end = 360000, int timeout_sec = 30); /** diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index 9e78ddc8..338a3e99 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -47,6 +47,41 @@ enum timestamp_mode { TIME_FROM_PTP_1588 }; +enum multipurpose_io_mode { + mio_mode_UNSPEC = 0, + mio_mode_OFF = 1, + mio_INPUT_NMEA_UART, + mio_OUTPUT_FROM_INTERNAL_OSC, + mio_OUTPUT_FROM_SYNC_PULSE_IN, + mio_OUTPUT_FROM_PTP_1588, + mio_OUTPUT_FROM_ENCODER_ANGLE +}; + +enum nmea_baud_rate { + BAUD_UNSPEC = 0, + BAUD_9600 = 1, + BAUD_115200 +}; + +enum nmea_in_polarity { + nmea_polarity_UNSPEC = 0, + nmea_polarity_ACTIVE_HIGH = 1, + nmea_polarity_ACTIVE_LOW +}; + +enum sync_pulse_in_polarity { + sync_pulse_in_UNSPEC = 0, + sync_pulse_in_ACTIVE_HIGH = 1, + sync_pulse_in_ACTIVE_LOW +}; + +enum nmea_ignore_valid_char{ + nmea_val_char_UNSPEC = 0, + nmea_val_char_ignore = 1, + nmea_val_char_not_ignore +}; + + enum configuration_version { FW_2_0 = 3 }; @@ -158,6 +193,98 @@ sensor_info metadata_from_json(const std::string& json_file); */ std::string to_string(const sensor_info& metadata); +/** + * Get string representation of a multipurpose_io_mode + * @param multipurpose_io_mode + * @return string representation of the multipurpose_io_mode + */ +std::string to_string(multipurpose_io_mode v); + +/** + * Get multipurpose_io_mode from string + * @param string + * @return multipurpose_io_mode corresponding to the string, or 0 on error + */ +multipurpose_io_mode multipurpose_io_mode_of_string(const std::string& s); + +/** + * Get string representation of a nmea_baud_rate + * @param nmea_baud_rate + * @return string representation of the nmea_baud_rate + */ +std::string to_string(nmea_baud_rate v); + +/** + * Get nmea_baud_rate from string + * @param string + * @return nmea_baud_rate corresponding to the string, or 0 on error + */ +nmea_baud_rate nmea_baud_rate_of_string(const std::string& s); + +/** + * Get string representation of a nmea_in_polarity + * @param nmea_in_polarity + * @return string representation of the nmea_in_polarity + */ +std::string to_string(nmea_in_polarity v); + +/** + * Get nmea_in_polarity from string + * @param string + * @return nmea_in_polarity corresponding to the string, or 0 on error + */ +nmea_in_polarity nmea_in_polarity_of_string(const std::string& s); + +/** + * Get string representation of a sync_pulse_in_polarity + * @param sync_pulse_in_polarity + * @return string representation of the sync_pulse_in_polarity + */ +std::string to_string(sync_pulse_in_polarity v); + +/** + * Get sync_pulse_in_polarity from string + * @param string + * @return sync_pulse_in_polarity corresponding to the string, or 0 on error + */ +sync_pulse_in_polarity sync_pulse_in_polarity_of_string(const std::string& s); + + +/** + * Get string representation of a nmea_ignore_valid_char + * @param nmea_ignore_valid_char + * @return string representation of the nmea_ignore_valid_char + */ +std::string to_string(nmea_ignore_valid_char v); + +/** + * Get nmea_ignore_valid_char from string + * @param string + * @return nmea_ignore_valid_char corresponding to the string, or 0 on error + */ +nmea_ignore_valid_char nmea_ignore_valid_char_of_string(const std::string& s); + +/** + * Get nmea_leap_seconds from string + * @param string + * @return nmea_leap_seconds corresponding to the string, or -999999 on error + */ +int nmea_leap_seconds_of_string(const std::string& s); + +/** + * Get azimuth_window value from string + * @param string + * @return azimuth_window corresponding to the string + */ +int azimuth_window_of_string(const std::string& s); + +/** + * Get string representation of a azimuth_window_start and azimuth_window_end + * @param int azimuth_window_start, int azimuth_window_end + * @return string representation of the azimuth_window + */ +std::string azimuth_window_to_string(const int start, const int end); + /** * Table of accessors for extracting data from imu and lidar packets. * diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 629c14d0..941ff25d 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -312,6 +312,14 @@ std::shared_ptr init_client(const std::string& hostname, const std::string& udp_dest_host, lidar_mode mode, timestamp_mode ts_mode, int lidar_port, int imu_port, + multipurpose_io_mode io_mode, + nmea_baud_rate baud, + nmea_ignore_valid_char nmea_ignore, + nmea_in_polarity nmea_polarity, + int nmea_leap_seconds, + sync_pulse_in_polarity sync_pulse_pol, + int azimuth_window_start, + int azimuth_window_end, int timeout_sec) { auto cli = init_client(hostname, lidar_port, imu_port); if (!cli) return std::shared_ptr(); @@ -357,6 +365,41 @@ std::shared_ptr init_client(const std::string& hostname, success &= res == "set_config_param"; } + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "multipurpose_io_mode", to_string(io_mode)}, + res); + success &= res == "set_config_param"; + + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "nmea_baud_rate", to_string(baud)}, + res); + success &= res == "set_config_param"; + + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "nmea_ignore_valid_char", to_string(nmea_ignore)}, + res); + success &= res == "set_config_param"; + + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "nmea_in_polarity", to_string(nmea_polarity)}, + res); + success &= res == "set_config_param"; + + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "sync_pulse_in_polarity", to_string(sync_pulse_pol)}, + res); + success &= res == "set_config_param"; + + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "nmea_leap_seconds", std::to_string(nmea_leap_seconds)}, + res); + success &= res == "set_config_param"; + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "azimuth_window", + azimuth_window_to_string(azimuth_window_start, azimuth_window_end)}, + res); + success &= res == "set_config_param"; + success &= do_tcp_cmd(sock_fd, {"reinitialize"}, res); success &= res == "reinitialize"; diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index 01af17e9..7855c798 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -33,6 +33,42 @@ const std::array, 3> {TIME_FROM_SYNC_PULSE_IN, "TIME_FROM_SYNC_PULSE_IN"}, {TIME_FROM_PTP_1588, "TIME_FROM_PTP_1588"}}}; +const std::array, 7> + multipurpose_io_mode_strings = { + {{mio_mode_UNSPEC, "UNSPEC"}, + {mio_mode_OFF, "OFF"}, + {mio_INPUT_NMEA_UART, "INPUT_NMEA_UART"}, + {mio_OUTPUT_FROM_INTERNAL_OSC, "OUTPUT_FROM_INTERNAL_OSC"}, + {mio_OUTPUT_FROM_SYNC_PULSE_IN, "OUTPUT_FROM_SYNC_PULSE_IN"}, + {mio_OUTPUT_FROM_PTP_1588, "OUTPUT_FROM_PTP_1588"}, + {mio_OUTPUT_FROM_ENCODER_ANGLE, "OUTPUT_FROM_ENCODER_ANGLE"}}}; + + +const std::array, 3> + nmea_baud_rate_strings = { + {{BAUD_UNSPEC, "BAUD_UNSPEC"}, + {BAUD_9600, "BAUD_9600"}, + {BAUD_115200, "BAUD_115200"}}}; + +const std::array, 3> + nmea_in_polarity_strings = { + {{nmea_polarity_UNSPEC, "nmea_polarity_UNSPEC"}, + {nmea_polarity_ACTIVE_HIGH, "ACTIVE_HIGH"}, + {nmea_polarity_ACTIVE_LOW, "ACTIVE_LOW"}}}; + +const std::array, 3> + sync_pulse_in_polarity_strings = { + {{sync_pulse_in_UNSPEC, "sync_pulse_in_UNSPEC"}, + {sync_pulse_in_ACTIVE_HIGH, "ACTIVE_HIGH"}, + {sync_pulse_in_ACTIVE_LOW, "ACTIVE_LOW"}}}; + +const std::array, 3> + nmea_ignore_valid_char_strings = { + {{nmea_val_char_UNSPEC, "nmea_val_char_UNSPEC"}, + {nmea_val_char_ignore, "0"}, + {nmea_val_char_not_ignore, "1"}}}; + + } // namespace bool operator==(const data_format& lhs, const data_format& rhs) { @@ -195,6 +231,129 @@ timestamp_mode timestamp_mode_of_string(const std::string& s) { return res == end ? timestamp_mode(0) : res->first; } +std::string to_string(multipurpose_io_mode mode) { + auto end = multipurpose_io_mode_strings.end(); + auto res = std::find_if(multipurpose_io_mode_strings.begin(), end, + [&](const std::pair& p) { + return p.first == mode; + }); + return res == end ? "UNKNOWN" : res->second; +} + +multipurpose_io_mode multipurpose_io_mode_of_string(const std::string& s) { + auto end = multipurpose_io_mode_strings.end(); + auto res = std::find_if(multipurpose_io_mode_strings.begin(), end, + [&](const std::pair& p) { + return p.second == s; + }); + return res == end ? multipurpose_io_mode(0) : res->first; +} + +std::string to_string(nmea_baud_rate baud) { + auto end = nmea_baud_rate_strings.end(); + auto res = std::find_if(nmea_baud_rate_strings.begin(), end, + [&](const std::pair& p) { + return p.first == baud; + }); + + return res == end ? "UNKNOWN" : res->second; +} + +nmea_baud_rate nmea_baud_rate_of_string(const std::string& s) { + auto end = nmea_baud_rate_strings.end(); + auto res = std::find_if(nmea_baud_rate_strings.begin(), end, + [&](const std::pair& p) { + return p.second == s; + }); + + return res == end ? nmea_baud_rate(0) : res->first; +} + +std::string to_string(nmea_in_polarity polarity) { + auto end = nmea_in_polarity_strings.end(); + auto res = std::find_if(nmea_in_polarity_strings.begin(), end, + [&](const std::pair& p) { + return p.first == polarity; + }); + + return res == end ? "UNKNOWN" : res->second; +} + +nmea_in_polarity nmea_in_polarity_of_string(const std::string& s) { + auto end = nmea_in_polarity_strings.end(); + auto res = std::find_if(nmea_in_polarity_strings.begin(), end, + [&](const std::pair& p) { + return p.second == s; + }); + + return res == end ? nmea_in_polarity(0) : res->first; +} + +std::string to_string(sync_pulse_in_polarity polarity) { + auto end = sync_pulse_in_polarity_strings.end(); + auto res = std::find_if(sync_pulse_in_polarity_strings.begin(), end, + [&](const std::pair& p) { + return p.first == polarity; + }); + + return res == end ? "UNKNOWN" : res->second; +} + +sync_pulse_in_polarity sync_pulse_in_polarity_of_string(const std::string& s) { + auto end = sync_pulse_in_polarity_strings.end(); + auto res = std::find_if(sync_pulse_in_polarity_strings.begin(), end, + [&](const std::pair& p) { + return p.second == s; + }); + + return res == end ? sync_pulse_in_polarity(0) : res->first; +} + +std::string to_string(nmea_ignore_valid_char mode) { + auto end = nmea_ignore_valid_char_strings.end(); + auto res = std::find_if(nmea_ignore_valid_char_strings.begin(), end, + [&](const std::pair& p) { + return p.first == mode; + }); + + return res == end ? "UNKNOWN" : res->second; +} + +nmea_ignore_valid_char nmea_ignore_valid_char_of_string(const std::string& s) { + auto end = nmea_ignore_valid_char_strings.end(); + auto res = std::find_if(nmea_ignore_valid_char_strings.begin(), end, + [&](const std::pair& p) { + return p.second == s; + }); + + return res == end ? nmea_ignore_valid_char(0) : res->first; +} + +int nmea_leap_seconds_of_string(const std::string& s) +{ + + return s == "0" ? 0 : (std::atoi(s.c_str()) == 0 ? -999999 :std::atoi(s.c_str())); + +} + +int azimuth_window_of_string(const std::string& s) +{ + if (s != "0" && std::atoi(s.c_str()) == 0) + return -999999; + else if (std::atoi(s.c_str()) >= 0 && std::atoi(s.c_str()) <= 360000) + return std::atoi(s.c_str()); + else + return -999999; +} + +std::string azimuth_window_to_string(const int start, const int end) +{ + if (start >=0 && start <= 360000 && end >=0 && end <= 360000) + return "[" + std::to_string(start) + "," + std::to_string(end) + "]"; + else + return "[0,360000]"; +} + sensor_info parse_metadata(const std::string& meta) { Json::Value root{}; Json::CharReaderBuilder builder{}; diff --git a/ouster_ros/ouster.launch b/ouster_ros/ouster.launch index 39323766..91e092dc 100644 --- a/ouster_ros/ouster.launch +++ b/ouster_ros/ouster.launch @@ -6,12 +6,22 @@ - + - + + + + + + + + + + + @@ -21,6 +31,14 @@ + + + + + + + + @@ -40,6 +58,10 @@ + + + diff --git a/ouster_ros/src/os_node.cpp b/ouster_ros/src/os_node.cpp index 72d324ae..d45b40db 100644 --- a/ouster_ros/src/os_node.cpp +++ b/ouster_ros/src/os_node.cpp @@ -125,12 +125,23 @@ int main(int argc, char** argv) { auto replay = nh.param("replay", false); auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); + auto multipurpose_io_mode_arg = nh.param("multipurpose_io_mode", std::string()); + auto nmea_baud_rate_arg = nh.param("nmea_baud_rate", std::string()); + auto nmea_ignore_valid_char_arg = std::to_string(nh.param("nmea_ignore_valid_char", int())); + auto nmea_in_polarity_arg = nh.param("nmea_in_polarity", std::string()); + auto nmea_leap_seconds_str_arg = std::to_string(nh.param("nmea_leap_seconds", int())); + auto sync_pulse_in_polarity_arg = nh.param("sync_pulse_in_polarity", std::string()); + auto azimuth_window_start_str_arg = std::to_string(nh.param("azimuth_window_start", int())); + auto azimuth_window_end_str_arg = std::to_string(nh.param("azimuth_window_end", int())); // fall back to metadata file name based on hostname, if available auto meta_file = nh.param("metadata", std::string{}); if (!meta_file.size() && hostname.size()) meta_file = hostname + ".json"; + + // // set lidar mode from param + // sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; if (lidar_mode_arg.size()) { if (replay) ROS_WARN("Lidar mode set in replay mode. May be ignored"); @@ -142,7 +153,10 @@ int main(int argc, char** argv) { } } + + // // set timestamp mode from param + // sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; if (timestamp_mode_arg.size()) { if (replay) @@ -155,6 +169,202 @@ int main(int argc, char** argv) { } } + + // + // set multipurpose_io_mode from param + // + sensor::multipurpose_io_mode multipurpose_io_mode = sensor::mio_mode_UNSPEC; + + if (replay) { + if (multipurpose_io_mode_arg.size()) + ROS_WARN("Multipurpose I/O mode set in replay mode. Will be ignored"); + } + else { + if (multipurpose_io_mode_arg.size() == 0) { + ROS_WARN("Any setting for multipurpose I/O mode, used default OFF"); + multipurpose_io_mode = sensor::mio_mode_OFF; + } + else { + multipurpose_io_mode = sensor::multipurpose_io_mode_of_string(multipurpose_io_mode_arg); + if (!multipurpose_io_mode) { + ROS_ERROR("Invalid setting for multipurpose I/O mode %s", multipurpose_io_mode_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set nmea baud rate from param + // + sensor::nmea_baud_rate nmea_baud_rate = sensor::BAUD_UNSPEC; + + if (replay) { + if (nmea_baud_rate_arg.size()) + ROS_WARN("NMEA baud rate set in replay mode. Will be ignored"); + } + else { + if (not nmea_baud_rate_arg.size()) { + ROS_WARN("Any setting for NMEA baud rate, used default BAUD_9600"); + nmea_baud_rate = sensor::BAUD_9600; + } + else { + nmea_baud_rate = sensor::nmea_baud_rate_of_string(nmea_baud_rate_arg); + if (!nmea_baud_rate) { + ROS_ERROR("Invalid setting for NMEA baud rate %s", nmea_baud_rate_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set nmea polarity + // + sensor::nmea_in_polarity nmea_in_polarity = sensor::nmea_polarity_UNSPEC; + + if (replay) { + if (nmea_in_polarity_arg.size()) + ROS_WARN("NMEA input polarity set in replay mode. Will be ignored"); + } + else { + if (not nmea_in_polarity_arg.size()) { + ROS_WARN("Any setting for NMEA input polarity, used default ACTIVE_LOW"); + nmea_in_polarity = sensor::nmea_polarity_ACTIVE_LOW; + } + else { + nmea_in_polarity = sensor::nmea_in_polarity_of_string(nmea_in_polarity_arg); + if (!nmea_in_polarity) { + ROS_ERROR("Invalid setting for NMEA input polarity %s", nmea_in_polarity_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set sync pulse in polarity + // + sensor::sync_pulse_in_polarity sync_pulse_in_polarity = sensor::sync_pulse_in_UNSPEC; + + if (replay) { + if (sync_pulse_in_polarity_arg.size()) + ROS_WARN("Sync pulse input polarity set in replay mode. Will be ignored"); + } + else { + if (not sync_pulse_in_polarity_arg.size()) { + ROS_WARN("Any setting for sync pulse input polarity, used default ACTIVE_HIGH"); + sync_pulse_in_polarity = sensor::sync_pulse_in_ACTIVE_HIGH; + } + else { + sync_pulse_in_polarity = sensor::sync_pulse_in_polarity_of_string(sync_pulse_in_polarity_arg); + if (!sync_pulse_in_polarity) { + ROS_ERROR("Invalid sync pulse input polarity %s", sync_pulse_in_polarity_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set nmea ignore valid char + // + sensor::nmea_ignore_valid_char nmea_ignore_valid_char = sensor::nmea_val_char_UNSPEC; + + if (replay) { + if (nmea_ignore_valid_char_arg.size()) + ROS_WARN("NMEA ignore valid char set in replay mode. Will be ignored"); + } + else { + if (not nmea_ignore_valid_char_arg.size()) { + ROS_WARN("Any setting for NMEA ignore valid char, used default IGNORE (0)"); + nmea_ignore_valid_char = sensor::nmea_val_char_ignore; + } + else { + nmea_ignore_valid_char = sensor::nmea_ignore_valid_char_of_string(nmea_ignore_valid_char_arg); + if (!nmea_ignore_valid_char) { + ROS_ERROR("Invalid setting for NMEA ignore valid char %s", nmea_ignore_valid_char_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set nmea leap seconds + // + int nmea_leap_seconds = 0; + + if (replay) { + if (nmea_leap_seconds_str_arg.size()) + ROS_WARN("NMEA leap seconds set in replay mode. Will be ignored"); + } + else { + if (not nmea_leap_seconds_str_arg.size()) { + ROS_WARN("Any setting for NMEA leap seconds, used default 0"); + nmea_leap_seconds = 0; + } + else { + nmea_leap_seconds = sensor::nmea_leap_seconds_of_string(nmea_leap_seconds_str_arg); + if (nmea_leap_seconds == -999999) { + ROS_WARN("Invalid setting for NMEA leap seconds %s", nmea_leap_seconds_str_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set azimuth window start + // + int azimuth_window_start = 0; + + if (replay) { + if (azimuth_window_start_str_arg.size()) + ROS_WARN("Azimuth window start set in replay mode. Will be ignored"); + } + else { + if (not azimuth_window_start_str_arg.size()) { + ROS_WARN("Any setting for azimuth window start, used default 0"); + azimuth_window_start = 0; + } + else { + azimuth_window_start = sensor::azimuth_window_of_string(azimuth_window_start_str_arg); + if (azimuth_window_start == -999999) { + ROS_ERROR("Invalid setting for azimuth window start %s", azimuth_window_start_str_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + // + // set azimuth window end + // + int azimuth_window_end = 360000; + + if (replay) { + if (azimuth_window_end_str_arg.size()) + ROS_WARN("Azimuth window end set in replay mode. Will be ignored"); + } + else { + if (not azimuth_window_end_str_arg.size()) { + ROS_WARN("Any setting for azimuth window end, used default 360000"); + azimuth_window_end = 360000; + } + else { + azimuth_window_end = sensor::azimuth_window_of_string(azimuth_window_end_str_arg); + if (azimuth_window_end == -999999) { + ROS_ERROR("Invalid setting for azimuth window end %s", azimuth_window_end_str_arg.c_str()); + return EXIT_FAILURE; + } + } + } + + + + + if (!replay && (!hostname.size() || !udp_dest.size())) { ROS_ERROR("Must specify both hostname and udp destination"); return EXIT_FAILURE; @@ -187,7 +397,15 @@ int main(int argc, char** argv) { ROS_INFO("Waiting for sensor to initialize ..."); auto cli = sensor::init_client(hostname, udp_dest, lidar_mode, - timestamp_mode, lidar_port, imu_port); + timestamp_mode, lidar_port, imu_port, + multipurpose_io_mode, + nmea_baud_rate, + nmea_ignore_valid_char, + nmea_in_polarity, + nmea_leap_seconds, + sync_pulse_in_polarity, + azimuth_window_start, + azimuth_window_end); if (!cli) { ROS_ERROR("Failed to initialize sensor at: %s", hostname.c_str());