Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions modules/canbus/vehicle/lincoln/lincoln_controller.cc
Original file line number Diff line number Diff line change
Expand Up @@ -665,14 +665,13 @@ bool LincolnController::CheckResponse(const int32_t flags, bool need_wait) {
bool is_eps_online = false;
bool is_vcu_online = false;
bool is_esp_online = false;
bool check_ok = false;

do {
if (message_manager_->GetChassisDetail(&chassis_detail) != ErrorCode::OK) {
AERROR << "get chassis detail failed.";
return false;
}
check_ok = true;
bool check_ok = true;
if (flags & CHECK_RESPONSE_STEER_UNIT_FLAG) {
is_eps_online = chassis_detail.has_check_response() &&
chassis_detail.check_response().has_is_eps_online() &&
Expand Down
6 changes: 3 additions & 3 deletions modules/drivers/gnss/include/gnss/novatel_messages.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@
// http://www.novatel.com/assets/Documents/Manuals/om-20000129.pdf
// http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf

#ifndef DRIVERS_GNSS_NOVATEL_MESSAGES_H
#define DRIVERS_GNSS_NOVATEL_MESSAGES_H
#ifndef MODULES_DRIVERS_GNSS_NOVATEL_MESSAGES_H_
#define MODULES_DRIVERS_GNSS_NOVATEL_MESSAGES_H_

#include <stdint.h>

Expand Down Expand Up @@ -353,4 +353,4 @@ inline ImuParameter get_imu_parameter(ImuType type) {
} // namespace drivers
} // namespace apollo

#endif // DRIVERS_GNSS_NOVATEL_MESSAGES_H
#endif // MODULES_DRIVERS_GNSS_NOVATEL_MESSAGES_H_
6 changes: 3 additions & 3 deletions modules/drivers/gnss/include/gnss/parser.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
* limitations under the License.
*****************************************************************************/

#ifndef DRIVERS_GNSS_PARSER_H
#define DRIVERS_GNSS_PARSER_H
#ifndef MODULES_DRIVERS_GNSS_PARSER_H_
#define MODULES_DRIVERS_GNSS_PARSER_H_

#include <stdint.h>

Expand Down Expand Up @@ -103,4 +103,4 @@ class Parser {
} // namespace drivers
} // namespace apollo

#endif // DRIVERS_GNSS_PARSER_H
#endif // MODULES_DRIVERS_GNSS_PARSER_H_
6 changes: 3 additions & 3 deletions modules/drivers/gnss/include/gnss/stream.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@

// This defines an stream interface for communication via USB, Ethernet, etc.

#ifndef DRIVERS_GNSS_STREAM_H
#define DRIVERS_GNSS_STREAM_H
#ifndef MODULES_DRIVERS_GNSS_STREAM_H_
#define MODULES_DRIVERS_GNSS_STREAM_H_

#include <ros/ros.h>
#include <stdint.h>
Expand Down Expand Up @@ -107,4 +107,4 @@ class Stream {
} // namespace drivers
} // namespace apollo

#endif // DRIVERS_GNSS_STREAM_H
#endif // MODULES_DRIVERS_GNSS_STREAM_H_
7 changes: 3 additions & 4 deletions modules/drivers/gnss/include/gnss/utils.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,12 @@
* limitations under the License.
*****************************************************************************/

#ifndef DRIVERS_GNSS_UTILS_H
#define DRIVERS_GNSS_UTILS_H
#ifndef MODULES_DRIVERS_GNSS_UTILS_H_
#define MODULES_DRIVERS_GNSS_UTILS_H_

#include <string>

#include "proto/config.pb.h"
//#include "modules/drivers/gnss/proto/config.pb.h"

namespace apollo {
namespace drivers {
Expand All @@ -34,4 +33,4 @@ std::string encode_base64(const std::string& in);
} // namespace drivers
} // namespace apollo

#endif // DRIVERS_GNSS_UTILS_H
#endif // MODULES_DRIVERS_GNSS_UTILS_H_
6 changes: 3 additions & 3 deletions modules/drivers/gnss/include/util/macros.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
// Commonly-used macro definitions. Some of them are copied from
// https://chromium.googlesource.com/chromium/src/base/+/master/macros.h

#ifndef APOLLO_DRIVERS_GNSS_INCLUDE_UTIL_MACROS_H
#define APOLLO_DRIVERS_GNSS_INCLUDE_UTIL_MACROS_H
#ifndef MODULES_DRIVERS_GNSS_INCLUDE_UTIL_MACROS_H_
#define MODULES_DRIVERS_GNSS_INCLUDE_UTIL_MACROS_H_

#include <cstddef>

Expand Down Expand Up @@ -65,4 +65,4 @@ constexpr size_t array_size(T (&)[N]) {

} // namespace apollo

#endif // APOLLO_DRIVERS_GNSS_INCLUDE_UTIL_MACROS_H
#endif // MODULES_DRIVERS_GNSS_INCLUDE_UTIL_MACROS_H_
7 changes: 4 additions & 3 deletions modules/drivers/gnss/include/util/time_conversion.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,11 @@
// Converts GPS timestamp from/to UNIX system timestamp.
// This helper is considering leap second.

#ifndef APOLLO_DRIVERS_GNSS_TIME_CONVERSION_H
#define APOLLO_DRIVERS_GNSS_TIME_CONVERSION_H
#ifndef MODULES_DRIVERS_GNSS_TIME_CONVERSION_H_
#define MODULES_DRIVERS_GNSS_TIME_CONVERSION_H_

#include <stdint.h>

#include "macros.h"

namespace apollo {
Expand Down Expand Up @@ -139,4 +140,4 @@ T gps2unix(T gps_seconds) {
} // namespace drivers
} // namespace apollo

#endif // APOLLO_DRIVERS_GNSS_TIME_CONVERSION_H
#endif // MODULES_DRIVERS_GNSS_TIME_CONVERSION_H_
7 changes: 4 additions & 3 deletions modules/drivers/gnss/src/data_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -306,6 +306,7 @@ void DataParser::publish_corrimu_pb_message(const MessagePtr message) {
<< " s.");
_imu_publisher.publish(imu);
}
}
}
}

} // namespace gnss
} // namespace drivers
} // namespace apollo
6 changes: 3 additions & 3 deletions modules/drivers/gnss/src/data_parser.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
* limitations under the License.
*****************************************************************************/

#ifndef DRIVERS_GNSS_DATA_PARSER_H
#define DRIVERS_GNSS_DATA_PARSER_H
#ifndef MODULES_DRIVERS_GNSS_DATA_PARSER_H_
#define MODULES_DRIVERS_GNSS_DATA_PARSER_H_

#include <memory>

Expand Down Expand Up @@ -75,4 +75,4 @@ class DataParser {
} // namespace drivers
} // namespace apollo

#endif
#endif // MODULES_DRIVERS_GNSS_DATA_PARSER_H_
3 changes: 1 addition & 2 deletions modules/drivers/gnss/src/impl/tcp_stream.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -282,7 +282,6 @@ size_t TcpStream::read(uint8_t* buffer, size_t max_length) {
}

size_t TcpStream::write(const uint8_t* buffer, size_t length) {
ssize_t nsent = 0;
size_t total_nsent = 0;

if (_status != Stream::Status::CONNECTED) {
Expand All @@ -294,7 +293,7 @@ size_t TcpStream::write(const uint8_t* buffer, size_t length) {
}

while (length > 0) {
nsent = ::send(_sockfd, buffer, length, 0);
ssize_t nsent = ::send(_sockfd, buffer, length, 0);
if (nsent < 0) {
if (errno == EINTR) {
continue;
Expand Down
6 changes: 3 additions & 3 deletions modules/drivers/gnss/src/impl/udp_stream.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,6 @@ size_t UdpStream::read(uint8_t* buffer, size_t max_length) {
}

size_t UdpStream::write(const uint8_t* data, size_t length) {
ssize_t nsent = 0;
size_t total_nsent = 0;
struct sockaddr_in peer_sockaddr;
bzero(&peer_sockaddr, sizeof(peer_sockaddr));
Expand All @@ -207,8 +206,9 @@ size_t UdpStream::write(const uint8_t* data, size_t length) {
peer_sockaddr.sin_addr.s_addr = _peer_addr;

while (length > 0) {
nsent = ::sendto(_sockfd, data, length, 0, (struct sockaddr*)&peer_sockaddr,
(socklen_t)sizeof(peer_sockaddr));
ssize_t nsent =
::sendto(_sockfd, data, length, 0, (struct sockaddr*)&peer_sockaddr,
(socklen_t)sizeof(peer_sockaddr));
if (nsent < 0) { // error
if (errno == EINTR) {
continue;
Expand Down
88 changes: 44 additions & 44 deletions modules/drivers/gnss/src/parser_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,70 +14,70 @@
* limitations under the License.
*****************************************************************************/

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include "gnss/parser.h"
#include "data_parser.h"
#include "gnss/parser.h"

namespace apollo {
namespace drivers{
namespace drivers {
namespace gnss {

class ParserNodelet : public nodelet::Nodelet {
public:
ParserNodelet() {}
~ParserNodelet() {}
public:
ParserNodelet() {}
~ParserNodelet() {}

private:
virtual void onInit();
private:
virtual void onInit();

std::unique_ptr<DataParser> _data_parser;
std::unique_ptr<DataParser> _data_parser;
};

void ParserNodelet::onInit() {
ros::NodeHandle& nh = getPrivateNodeHandle();
std::string gnss_conf;
std::string raw_data_topic;
std::string gpgga_topic;
std::string corr_imu_topic;
std::string odometry_topic;
std::string gnss_status_topic;
std::string ins_status_topic;
ros::NodeHandle& nh = getPrivateNodeHandle();
std::string gnss_conf;
std::string raw_data_topic;
std::string gpgga_topic;
std::string corr_imu_topic;
std::string odometry_topic;
std::string gnss_status_topic;
std::string ins_status_topic;

nh.param("gnss_conf", gnss_conf, std::string("./conf/gnss_conf.txt"));
nh.param("raw_data_topic", raw_data_topic, std::string("/apollo/sensor/gnss/raw_data"));
nh.param("gpgga_topic", gpgga_topic, std::string("/apollo/sensor/gnss/gpgga"));
nh.param("corr_imu_topic", corr_imu_topic, std::string("/apollo/sensor/gnss/corrected_imu"));
nh.param("odometry_topic", odometry_topic, std::string("/apollo/sensor/gnss/odometry"));
nh.param("gnss_status_topic", gnss_status_topic, std::string("/apollo/sensor/gnss/gnss_status"));
nh.param("ins_status_topic", ins_status_topic, std::string("/apollo/sensor/gnss/ins_status"));
nh.param("gnss_conf", gnss_conf, std::string("./conf/gnss_conf.txt"));
nh.param("raw_data_topic", raw_data_topic,
std::string("/apollo/sensor/gnss/raw_data"));
nh.param("gpgga_topic", gpgga_topic,
std::string("/apollo/sensor/gnss/gpgga"));
nh.param("corr_imu_topic", corr_imu_topic,
std::string("/apollo/sensor/gnss/corrected_imu"));
nh.param("odometry_topic", odometry_topic,
std::string("/apollo/sensor/gnss/odometry"));
nh.param("gnss_status_topic", gnss_status_topic,
std::string("/apollo/sensor/gnss/gnss_status"));
nh.param("ins_status_topic", ins_status_topic,
std::string("/apollo/sensor/gnss/ins_status"));

_data_parser.reset(new DataParser(
nh,
raw_data_topic,
gpgga_topic,
corr_imu_topic,
odometry_topic,
gnss_status_topic,
ins_status_topic));
if (!_data_parser->init(gnss_conf)) {
ROS_ERROR("Init parser nodelet failed.");
ROS_ERROR_STREAM("Init parser nodelet failed.");
return;
}
ROS_INFO("Init parser nodelet success.");
_data_parser.reset(new DataParser(nh, raw_data_topic, gpgga_topic,
corr_imu_topic, odometry_topic,
gnss_status_topic, ins_status_topic));
if (!_data_parser->init(gnss_conf)) {
ROS_ERROR("Init parser nodelet failed.");
ROS_ERROR_STREAM("Init parser nodelet failed.");
return;
}
ROS_INFO("Init parser nodelet success.");
}

}
}
}
} // namespace gnss
} // namespace drivers
} // namespace apollo

// Register this plugin with pluginlib. Names must match nodelet_gnss.xml.
//
// parameters: package, class name, class type, base class type
PLUGINLIB_DECLARE_CLASS(gnss_driver, ParserNodelet,
apollo::drivers::gnss::ParserNodelet, nodelet::Nodelet);

apollo::drivers::gnss::ParserNodelet, nodelet::Nodelet);
17 changes: 2 additions & 15 deletions modules/drivers/gnss/src/raw_stream.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -378,19 +378,6 @@ bool RawStream::logout() {
return true;
}

std::string RawStream::status_string(Stream::Status status) {
switch (status) {
case Stream::Status::DISCONNECTED:
return std::string("disconnected");

case Stream::Status::ERROR:
return std::string("error");

default:
return std::string("ok.");
}
}

void RawStream::stream_status_check() {
bool status_report = false;
apollo::common::gnss_status::StreamStatus_Type report_stream_status;
Expand Down Expand Up @@ -449,7 +436,6 @@ void RawStream::ntrip_spin() {
return;
}
while (ros::ok()) {
size_t ret = 0;
size_t length = _in_rtk_stream->read(_buffer_ntrip, BUFFER_SIZE);
if (length > 0) {
if (_rtk_software_solution) {
Expand All @@ -465,7 +451,7 @@ void RawStream::ntrip_spin() {
if (_out_rtk_stream == nullptr) {
continue;
}
ret = _out_rtk_stream->write(_buffer_ntrip, length);
size_t ret = _out_rtk_stream->write(_buffer_ntrip, length);
if (ret != length) {
ROS_ERROR_STREAM("Expect write out rtk stream bytes "
<< length << " but got " << ret);
Expand All @@ -474,6 +460,7 @@ void RawStream::ntrip_spin() {
}
}
}

} // namespace gnss
} // namespace drivers
} // namespace apollo
15 changes: 8 additions & 7 deletions modules/drivers/gnss/src/raw_stream.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@
* limitations under the License.
*****************************************************************************/

#ifndef DRIVERS_GNSS_RAW_STREAM_H
#define DRIVERS_GNSS_RAW_STREAM_H
#ifndef MODULES_DRIVERS_GNSS_RAW_STREAM_H_
#define MODULES_DRIVERS_GNSS_RAW_STREAM_H_

#include <memory>
#include <thread>
Expand Down Expand Up @@ -52,7 +52,6 @@ class RawStream {
bool login();
bool logout();
void stream_status_check();
std::string status_string(Stream::Status status);

static constexpr size_t BUFFER_SIZE = 2048;
uint8_t _buffer[BUFFER_SIZE];
Expand Down Expand Up @@ -82,7 +81,9 @@ class RawStream {
std::unique_ptr<std::thread> _data_thread_ptr;
std::unique_ptr<std::thread> _ntrip_thread_ptr;
};
}
}
}
#endif

} // namespace apollo
} // namespace drivers
} // namespace gnss

#endif // MODULES_DRIVERS_GNSS_RAW_STREAM_H_
Loading