From e4c9f9b4896554ce3fbbec6f5f2fe8f66ebad59d Mon Sep 17 00:00:00 2001 From: X-lanni Date: Mon, 1 Sep 2025 19:07:59 +0800 Subject: [PATCH 1/7] refactor(agv_pro_base): Change serial_driver to boost/asio serial port library --- .../include/agv_pro_base/agv_pro_driver.h | 145 +++++++++- agv_pro_base/src/agv_pro_ros.cpp | 254 ++++++++++-------- 2 files changed, 275 insertions(+), 124 deletions(-) diff --git a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h index fa1512e..10cd4dd 100644 --- a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h +++ b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h @@ -2,7 +2,8 @@ #define AGV_PRO_DRIVER_H #include -#include "serial_driver/serial_driver.hpp" +#include +#include #include "rclcpp/rclcpp.hpp" @@ -13,7 +14,9 @@ #include #include -#define RECEIVE_DATA_SIZE 14 //The length of the data sent by the esp32 +#define SEND_DATA_SIZE 14 // Total bytes in a command frame to ESP32 +#define RECEIVE_FRAME_SIZE 32 // Total bytes in a frame from ESP32 +#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3) // Payload length (excluding header) extern std::array odom_pose_covariance; extern std::array odom_twist_covariance; @@ -21,20 +24,131 @@ extern std::array odom_twist_covariance; class AGV_PRO : public rclcpp::Node { public: + /** + * @brief Constructor + */ AGV_PRO(std::string node_name); + + /** + * @brief Destructor + */ ~AGV_PRO(); + private: + /** + * @brief Main control loop for the AGV. + */ void Control(); - void print_hex(const std::string& label, const std::vector& data, std::optional override_size = std::nullopt); + + /** + * @brief Print a vector of bytes in hexadecimal format to the ROS logger. + * + * @param[in] label A label to prepend to the printed data. + * @param[in] data The byte vector to print. + * @param[in] override_size Optional size to display instead of the full data length. + */ + void print_hex(const std::string& label, + const std::vector& data, + std::optional override_size = std::nullopt); + + /** + * @brief Send a serial frame to the AGV and optionally print it in hex. + * + * @param[in] frame The byte vector representing the serial frame to send. + * @param[in] debug If true, prints the transmitted frame using print_hex(). + */ void send_serial_frame(const std::vector& frame, bool debug); - void is_power_on(); - void set_auto_report(); + + /** + * @brief Query and print the current power status of the AGV. + * @return true if AGV is successfully power on; false otherwise. + */ + bool is_power_on(); + + /** + * @brief Enable or disable AGV auto-reporting + * @param[in] enable 0 = disable, 1 = enable + */ + void set_auto_report(bool enable); + + /** + * @brief Clear the serial port input and output buffers + * @param[in] fd File descriptor of the serial port + */ + void clearSerialBuffer(int fd); + + /** + * @brief Disable the DTR (Data Terminal Ready) and RTS (Request To Send) lines of the serial port + * @param[in] fd File descriptor of the serial port + */ + void disableDTR_RTS(int fd); + + /** + * @brief Read sensor and motor data from the AGV via serial port. + * @return true if data is successfully read and verified; false otherwise. + */ bool readData(); + + /** + * @brief Odometry publisher + * @param[in] dt Time difference (in seconds) since the last odometry update. + */ void publisherOdom(double dt); + + /** + * @brief Voltage publisher + */ void publisherVoltage(); + + /** + * @brief Callback for velocity command updates + * @param[in] msg The Twist message containing desired linear and angular velocities + */ void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** + * @brief Build a standard AGV serial frame with header, payload, and CRC. + * + * The frame has a fixed size of RECEIVE_DATA_SIZE, starts with 0xFE 0xFE 0x0B, + * includes a command ID and up to 8 payload bytes, and ends with a 16-bit CRC. + * + * @param[in] cmd_id The command ID for the serial frame. + * @param[in] payload The payload bytes to include (up to 8 bytes). + * @return A vector containing the complete serial frame ready to transmit. + */ std::vector build_serial_frame(uint8_t cmd_id, const std::vector& payload); - std::vector read_serial_response(const std::vector& expected_header, size_t payload_size, double timeout_sec); + + /** + * @brief Read a serial response from the AGV device, waiting for a specific header. + * + * This function reads bytes from the serial port until the expected header + * sequence is detected or the timeout expires. After detecting the header, + * it reads the remaining payload bytes along with a 2-byte CRC. + * + * @param[in] expected_header The byte sequence to identify the start of a valid frame. + * @param[in] payload_size The expected number of payload bytes following the header. + * @param[in] timeout_sec Maximum time (in seconds) to wait for the header. + * @return A vector containing the complete frame (header + payload + CRC). + * Returns an empty vector if a timeout occurs or the full payload is not received. + */ + std::vector read_serial_response( + const std::vector& expected_header, + size_t payload_size, + double timeout_sec); + + /** + * @brief Compute the CRC-16-IBM checksum for a byte array. + * + * This function calculates the CRC using the standard IBM polynomial 0xA001. + * + * @param[in] data Pointer to the byte array. + * @param[in] length Number of bytes to include in the CRC calculation. + * @return The computed 16-bit CRC value. + */ + uint16_t crc16_ibm(const uint8_t* data, size_t length); + + boost::asio::io_service io_; + std::unique_ptr serial_port_; std::string frame_id_of_odometry_; std::string child_frame_id_of_odometry_; @@ -63,6 +177,22 @@ private: float battery_voltage = 0.0f; + std::array odom_pose_covariance = { + {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9} }; + + std::array odom_twist_covariance = { + {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9} }; + rclcpp::Time currentTime, lastTime; rclcpp::TimerBase::SharedPtr control_timer_; rclcpp::Publisher::SharedPtr pub_odom; @@ -71,9 +201,6 @@ private: rclcpp::Subscription::SharedPtr cmd_sub; std::unique_ptr odomBroadcaster; - std::shared_ptr serial_driver_; - std::shared_ptr io_context_; - }; #endif \ No newline at end of file diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 829522a..4122a73 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -1,22 +1,6 @@ #include "agv_pro_base/agv_pro_driver.h" -std::array odom_pose_covariance = { - {1e-9, 0, 0, 0, 0, 0, - 0, 1e-3, 1e-9, 0, 0, 0, - 0, 0, 1e6, 0, 0, 0, - 0, 0, 0, 1e6, 0, 0, - 0, 0, 0, 0, 1e6, 0, - 0, 0, 0, 0, 0, 1e-9} }; - -std::array odom_twist_covariance = { - {1e-9, 0, 0, 0, 0, 0, - 0, 1e-3, 1e-9, 0, 0, 0, - 0, 0, 1e6, 0, 0, 0, - 0, 0, 0, 1e6, 0, 0, - 0, 0, 0, 0, 1e6, 0, - 0, 0, 0, 0, 0, 1e-9} }; - -uint16_t crc16_ibm(const uint8_t* data, size_t length) { +uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) { uint16_t crc = 0xFFFF; for (size_t i = 0; i < length; ++i) { crc ^= static_cast(data[i]); @@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) { std::vector AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector& payload) { - std::vector frame(RECEIVE_DATA_SIZE, 0x00); + std::vector frame(SEND_DATA_SIZE, 0x00); frame[0] = 0xFE; frame[1] = 0xFE; frame[2] = 0x0B; @@ -62,8 +46,7 @@ void AGV_PRO::print_hex(const std::string& label, const std::vector& da void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) { try { - auto port = serial_driver_->port(); - size_t bytes_transmit_size = port->send(frame); + size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame)); if (debug) { print_hex("Sent", frame, bytes_transmit_size); } @@ -72,9 +55,11 @@ void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) } } -std::vector AGV_PRO::read_serial_response(const std::vector& expected_header, size_t payload_size, double timeout_sec) +std::vector AGV_PRO::read_serial_response( + const std::vector& expected_header, + size_t payload_size, + double timeout_sec) { - auto port = serial_driver_->port(); std::vector sliding_buf; uint8_t byte = 0; @@ -82,21 +67,24 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec); while ((this->now() - start_time) < timeout) { - std::vector temp_buf(1); - if (port->receive(temp_buf) == 1) { - byte = temp_buf[0]; + boost::asio::mutable_buffers_1 buf(&byte, 1); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + if (n == 1) { sliding_buf.push_back(byte); - if (sliding_buf.size() > expected_header.size()) { - sliding_buf.erase(sliding_buf.begin()); + sliding_buf.erase(sliding_buf.begin()); } - if (sliding_buf == expected_header) { - break; + break; } } } - + if (sliding_buf != expected_header) { RCLCPP_WARN(this->get_logger(), "Timeout waiting for header"); return {}; @@ -104,7 +92,20 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e size_t remain_len = payload_size + 2; std::vector remain_buf(remain_len); - if (port->receive(remain_buf) != remain_len) { + size_t total_read = 0; + + while (total_read < remain_len && (this->now() - start_time) < timeout) { + boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + total_read += n; + } + + if (total_read != remain_len) { RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload"); return {}; } @@ -115,22 +116,23 @@ std::vector AGV_PRO::read_serial_response(const std::vector& e return full_buf; } -void AGV_PRO::is_power_on(){ +bool AGV_PRO::is_power_on(){ auto power_query_frame = build_serial_frame(0x12, {}); send_serial_frame(power_query_frame,true); const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12}; - auto power_query_response = read_serial_response(expected_header, 8, 1.0); + RCLCPP_INFO(this->get_logger(),"1111111"); + auto power_query_response = read_serial_response(expected_header, 8, 12.0); print_hex("recv_buf", power_query_response); - if (power_query_response.size() != 14) return; + if (power_query_response.size() != 14) return false; uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13]; uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12); if (received_crc != computed_crc) { RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc); - return; + return false; } int is_poweron_status = static_cast(power_query_response[4]); @@ -140,19 +142,19 @@ void AGV_PRO::is_power_on(){ auto status_query_frame = build_serial_frame(0x10, {}); send_serial_frame(status_query_frame,true); - rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command + //rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x10}; auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds print_hex("recv_buf", status_query_response); - if (status_query_response.size() != 14) return; + if (status_query_response.size() != 14) return false; uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13]; uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12); if (received_crc != computed_crc) { RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc); - return; + return false; } int poweron_status = static_cast(status_query_response[4]); @@ -162,37 +164,61 @@ void AGV_PRO::is_power_on(){ case 1: status_msg = "Motor is operating normally."; RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return true; case 2: status_msg = "Emergency stop button is not released."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; case 3: status_msg = "Battery voltage is below 19.5V."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; case 4: status_msg = "CAN initialization error."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; case 5: status_msg = "Motor initialization error."; RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); - break; + return false; default: RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status); - break; + return false; } } - else + else{ RCLCPP_INFO(this->get_logger(), "Motor is operating normally."); + return true; + } } -void AGV_PRO::set_auto_report(){ - auto frame = build_serial_frame(0x23, {0x01}); +void AGV_PRO::set_auto_report(bool enable){ + auto frame = build_serial_frame(0x23, {static_cast(enable)}); send_serial_frame(frame,true); } +void AGV_PRO::clearSerialBuffer(int fd) { + if (::tcflush(fd, TCIOFLUSH) != 0) { + RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno)); + } else { + RCLCPP_INFO(this->get_logger(), "Serial buffer flushed."); + } +} + +void AGV_PRO::disableDTR_RTS(int fd) { + int status; + if (::ioctl(fd, TIOCMGET, &status) == 0) { + status &= ~(TIOCM_DTR | TIOCM_RTS); + if (::ioctl(fd, TIOCMSET, &status) != 0) { + RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno)); + } else { + RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully."); + } + } else { + RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno)); + } +} + void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { linearX = std::clamp(msg->linear.x, -1.5, 1.5); @@ -220,11 +246,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) std::vector data_vec(buf, buf + sizeof(buf)); - auto port = serial_driver_->port(); - try { - port->send(data_vec); + boost::asio::write(*serial_port_,boost::asio::buffer(data_vec)); // print_hex("Sent", data_vec);//debug } catch(const std::exception &ex) @@ -235,34 +259,46 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) bool AGV_PRO::readData() { - std::vector buf_header(1); std::vector buf_length(1); - std::vector data_buf(RECEIVE_DATA_SIZE-3); + std::vector data_buf(RECEIVE_PAYLOAD_SIZE); - auto port = serial_driver_->port(); + uint8_t byte = 0; + boost::system::error_code ec; while (true) { - size_t ret = port->receive(buf_header); - if (ret != 1 || buf_header[0] != 0xfe) { + size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + if (ret != 1 || byte != 0xfe) { continue; } - ret = port->receive(buf_header); - if (ret == 1 && buf_header[0] == 0xfe) { + ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + if (ret == 1 && byte == 0xfe) { break; } } - size_t ret = port->receive(buf_length); + size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } - if (buf_length[0] != 0x0b) { + if (buf_length[0] != RECEIVE_FRAME_SIZE-6) { RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); return false; } - ret = port->receive(data_buf); - if (ret != data_buf.size()) + ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec); + if (ec || ret != data_buf.size()) { RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload"); return false; @@ -271,7 +307,7 @@ bool AGV_PRO::readData() std::vector recv_buf; recv_buf.push_back(0xFE); recv_buf.push_back(0xFE); - recv_buf.push_back(0x0B); + recv_buf.push_back(RECEIVE_FRAME_SIZE-6); recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end()); // print_hex("recv_buf", recv_buf); //debug @@ -281,8 +317,8 @@ bool AGV_PRO::readData() return false; } - uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8); - uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12); + uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8); + uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2); if (received_crc != computed_crc) { RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc); @@ -345,12 +381,12 @@ void AGV_PRO::publisherOdom(double dt) odom.pose.pose.position.y = y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; - odom.pose.covariance = odom_pose_covariance; + odom.pose.covariance = this->odom_pose_covariance; odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.angular.z = vtheta; - odom.twist.covariance = odom_twist_covariance; + odom.twist.covariance = this->odom_twist_covariance; pub_odom->publish(odom); } @@ -401,64 +437,52 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) lastTime = this->get_clock()->now(); - drivers::serial_driver::SerialPortConfig config( - 1000000, - drivers::serial_driver::FlowControl::NONE, - drivers::serial_driver::Parity::NONE, - drivers::serial_driver::StopBits::ONE - ); - try{ - io_context_ = std::make_shared(1); - serial_driver_ = std::make_shared(*io_context_); - serial_driver_->init_port(device_name_, config); - serial_driver_->port()->open(); - - RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully"); - RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str()); - RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate()); + serial_port_ = std::make_unique(io_); - AGV_PRO::is_power_on(); - AGV_PRO::set_auto_report(); + serial_port_->open(device_name_); + serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000)); + serial_port_->set_option(boost::asio::serial_port_base::character_size(8)); + serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); + serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); + serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); + + int fd = serial_port_->native_handle(); + this->clearSerialBuffer(fd); + this->disableDTR_RTS(fd); + + RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully"); + RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str()); + + boost::asio::serial_port_base::baud_rate baud_option; + serial_port_->get_option(baud_option); + unsigned int current_baud = baud_option.value(); + RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud); } catch (const std::exception &ex){ RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what()); return; } - - control_timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), - std::bind(&AGV_PRO::Control, this) - ); - RCLCPP_INFO(this->get_logger(), "Control timer started"); + if (this->is_power_on()) { + this->set_auto_report(1); + + control_timer_ = this->create_wall_timer( + std::chrono::milliseconds(20), + std::bind(&AGV_PRO::Control, this) + ); + RCLCPP_INFO(this->get_logger(), "Control timer started"); + } + else { + RCLCPP_WARN(this->get_logger(), "Control timer not started."); + } } AGV_PRO::~AGV_PRO() -{ - std::array buf = { - 0xFE, 0xFE, 0x0b, 0x22, - 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00 - }; - - uint16_t crc = crc16_ibm(buf.data(), 12); - buf[12] = (crc >> 8) & 0xff; - buf[13] = crc & 0xff; - - std::vector data_vec(buf.begin(), buf.end()); - - auto port = serial_driver_->port(); - - try - { - port->send(data_vec); - } - catch(const std::exception &ex) - { - RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what()); - } - - serial_driver_->port()->close(); - RCLCPP_INFO(this->get_logger(),"Shutting down"); +{ + if (serial_port_ && serial_port_->is_open()) { + this->set_auto_report(0); + serial_port_->cancel(); + serial_port_->close(); + } } \ No newline at end of file From a29b104b9d7b1ed84126d3d99f322fcc460e3869 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Tue, 2 Sep 2025 19:06:41 +0800 Subject: [PATCH 2/7] feat(agv_pro_base): add ImuSensor publisher --- .../include/agv_pro_base/agv_pro_driver.h | 22 ++- agv_pro_base/src/agv_pro_ros.cpp | 160 +++++++++++++----- 2 files changed, 134 insertions(+), 48 deletions(-) diff --git a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h index 10cd4dd..deb81d0 100644 --- a/agv_pro_base/include/agv_pro_base/agv_pro_driver.h +++ b/agv_pro_base/include/agv_pro_base/agv_pro_driver.h @@ -14,8 +14,8 @@ #include #include -#define SEND_DATA_SIZE 14 // Total bytes in a command frame to ESP32 -#define RECEIVE_FRAME_SIZE 32 // Total bytes in a frame from ESP32 +#define SEND_DATA_SIZE 14 // Total bytes in a command frame to ESP32(version>=V1.0.8) +#define RECEIVE_FRAME_SIZE 31 // Total bytes in a frame from ESP32(version>=V1.0.8) #define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3) // Payload length (excluding header) extern std::array odom_pose_covariance; @@ -100,6 +100,11 @@ private: */ void publisherVoltage(); + /** + * @brief ImuSensor publisher + */ + void publisherImuSensor(); + /** * @brief Callback for velocity command updates * @param[in] msg The Twist message containing desired linear and angular velocities @@ -168,6 +173,18 @@ private: double linearY = 0.0; double angularZ = 0.0; + double ax= 0.0; + double ay= 0.0; + double az= 0.0; + + double wx= 0.0; + double wy= 0.0; + double wz= 0.0; + + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + int is_poweron_status = 0; int poweron_status = 0; @@ -200,6 +217,7 @@ private: rclcpp::Publisher::SharedPtr pub_voltage; rclcpp::Subscription::SharedPtr cmd_sub; + sensor_msgs::msg::Imu imu_data; std::unique_ptr odomBroadcaster; }; diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 4122a73..bfac77e 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -55,65 +55,87 @@ void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) } } +// std::vector AGV_PRO::read_serial_response( +// const std::vector& expected_header, +// size_t payload_size, +// double timeout_sec) +// { +// boost::system::error_code ec; +// std::vector sliding_buf; +// uint8_t byte = 0; + +// while (true) { +// size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); +// if (ec) { +// RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); +// break; +// } + +// if (ret > 0) { +// sliding_buf.push_back(byte); +// RCLCPP_INFO(this->get_logger(), "Recv: 0x%02X", byte); +// if (sliding_buf.size() > 1024) { +// sliding_buf.clear(); +// } +// } +// } +// } + std::vector AGV_PRO::read_serial_response( const std::vector& expected_header, size_t payload_size, double timeout_sec) { - std::vector sliding_buf; + std::vector buffer; uint8_t byte = 0; rclcpp::Time start_time = this->now(); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec); - while ((this->now() - start_time) < timeout) { + while (rclcpp::ok()) { boost::asio::mutable_buffers_1 buf(&byte, 1); boost::system::error_code ec; - size_t n = serial_port_->read_some(buf, ec); + size_t len = serial_port_->read_some(boost::asio::buffer(&byte,1), ec); if (ec) { RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); - return {}; + break; } - if (n == 1) { - sliding_buf.push_back(byte); - if (sliding_buf.size() > expected_header.size()) { - sliding_buf.erase(sliding_buf.begin()); + if (len > 0) { + buffer.push_back(byte); + //**log */ + RCLCPP_INFO(this->get_logger(), "recv byte: 0x%02X", byte); + std::string buf_str; + for (auto b : buffer) { + char tmp[5]; + snprintf(tmp, sizeof(tmp), "%02X ", b); + buf_str += tmp; } - if (sliding_buf == expected_header) { - break; + std::string header_str; + for (auto b : expected_header) { + char tmp[5]; + snprintf(tmp, sizeof(tmp), "%02X ", b); + header_str += tmp; + } + RCLCPP_INFO(this->get_logger(), "buffer: %s", buf_str.c_str()); + RCLCPP_INFO(this->get_logger(), "expected_header: %s", header_str.c_str()); + //**log */ + if (buffer.size() >= expected_header.size()) { + bool match = true; + for (size_t i=0;iget_logger(), "Timeout waiting for header"); - return {}; + if (buffer.empty()) { + RCLCPP_WARN(this->get_logger(), "No data received within timeout!"); + } else { + RCLCPP_INFO(this->get_logger(), "Final buffer size=%zu", buffer.size()); } - - size_t remain_len = payload_size + 2; - std::vector remain_buf(remain_len); - size_t total_read = 0; - - while (total_read < remain_len && (this->now() - start_time) < timeout) { - boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read); - boost::system::error_code ec; - size_t n = serial_port_->read_some(buf, ec); - if (ec) { - RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); - return {}; - } - total_read += n; - } - - if (total_read != remain_len) { - RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload"); - return {}; - } - - std::vector full_buf = expected_header; - full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end()); - - return full_buf; + return buffer; } bool AGV_PRO::is_power_on(){ @@ -121,7 +143,6 @@ bool AGV_PRO::is_power_on(){ send_serial_frame(power_query_frame,true); const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12}; - RCLCPP_INFO(this->get_logger(),"1111111"); auto power_query_response = read_serial_response(expected_header, 8, 12.0); print_hex("recv_buf", power_query_response); @@ -198,7 +219,7 @@ void AGV_PRO::set_auto_report(bool enable){ } void AGV_PRO::clearSerialBuffer(int fd) { - if (::tcflush(fd, TCIOFLUSH) != 0) { + if (tcflush(fd, TCIOFLUSH) < 0) { RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno)); } else { RCLCPP_INFO(this->get_logger(), "Serial buffer flushed."); @@ -292,8 +313,8 @@ bool AGV_PRO::readData() return false; } - if (buf_length[0] != RECEIVE_FRAME_SIZE-6) { - RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); + if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) { + //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); return false; } @@ -307,13 +328,13 @@ bool AGV_PRO::readData() std::vector recv_buf; recv_buf.push_back(0xFE); recv_buf.push_back(0xFE); - recv_buf.push_back(RECEIVE_FRAME_SIZE-6); + recv_buf.push_back(0x1C); recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end()); - // print_hex("recv_buf", recv_buf); //debug + //print_hex("recv_buf", recv_buf); //debug if (recv_buf[3] != 0x25) { - // RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); + //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug return false; } @@ -334,6 +355,18 @@ bool AGV_PRO::readData() battery_voltage = static_cast(recv_buf[9]) / 10.0f; enable_status = recv_buf[10]; + imu_data.linear_acceleration.x = static_cast((static_cast(recv_buf[11]) << 8) | recv_buf[12]) * 0.01; + imu_data.linear_acceleration.y = static_cast((static_cast(recv_buf[13]) << 8) | recv_buf[14]) * 0.01; + imu_data.linear_acceleration.z = static_cast((static_cast(recv_buf[15]) << 8) | recv_buf[16]) * 0.01; + + imu_data.angular_velocity.x = static_cast((static_cast(recv_buf[17]) << 8) | recv_buf[18]) * 0.01; + imu_data.angular_velocity.y = static_cast((static_cast(recv_buf[19]) << 8) | recv_buf[20]) * 0.01; + imu_data.angular_velocity.z = static_cast((static_cast(recv_buf[21]) << 8) | recv_buf[22]) * 0.01; + + roll = static_cast((static_cast(recv_buf[23]) << 8) | recv_buf[24]) * 0.01; + pitch = static_cast((static_cast(recv_buf[25]) << 8) | recv_buf[26]) * 0.01; + yaw = static_cast((static_cast(recv_buf[27]) << 8) | recv_buf[28]) * 0.01; + return true; } @@ -344,6 +377,40 @@ void AGV_PRO::publisherVoltage() pub_voltage->publish(voltage_msg); } +void AGV_PRO::publisherImuSensor() +{ + sensor_msgs::msg::Imu ImuSensor; + + ImuSensor.header.stamp = this->get_clock()->now(); + ImuSensor.header.frame_id = "imu_link"; + + tf2::Quaternion qua; + qua.setRPY(0, 0, yaw * M_PI / 180.0); + + ImuSensor.orientation.x = qua[0]; + ImuSensor.orientation.y = qua[1]; + ImuSensor.orientation.z = qua[2]; + ImuSensor.orientation.w = qua[3]; + + ImuSensor.angular_velocity.x = imu_data.angular_velocity.x; + ImuSensor.angular_velocity.y = imu_data.angular_velocity.y; + ImuSensor.angular_velocity.z = imu_data.angular_velocity.z; + + ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x; + ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y; + ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z; + + ImuSensor.orientation_covariance[0] = 1e6; + ImuSensor.orientation_covariance[4] = 1e6; + ImuSensor.orientation_covariance[8] = 1e-6; + + ImuSensor.angular_velocity_covariance[0] = 1e6; + ImuSensor.angular_velocity_covariance[4] = 1e6; + ImuSensor.angular_velocity_covariance[8] = 1e-6; + + pub_imu->publish(ImuSensor); +} + void AGV_PRO::publisherOdom(double dt) { currentTime = this->get_clock()->now(); @@ -405,6 +472,7 @@ void AGV_PRO::Control() publisherOdom(dt); // RCLCPP_INFO(this->get_logger(), "dt:%f", dt); publisherVoltage(); + publisherImuSensor(); } } From b19a0817dca13fb50be45fd8a6dc0e943e7ca328 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Wed, 3 Sep 2025 11:49:16 +0800 Subject: [PATCH 3/7] fix(agv_pro_base): Fixed the problem that ESP32 cannot read serial port data --- agv_pro_base/src/agv_pro_ros.cpp | 104 +++++++++++++------------------ 1 file changed, 42 insertions(+), 62 deletions(-) diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index bfac77e..2dfc219 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -55,87 +55,65 @@ void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) } } -// std::vector AGV_PRO::read_serial_response( -// const std::vector& expected_header, -// size_t payload_size, -// double timeout_sec) -// { -// boost::system::error_code ec; -// std::vector sliding_buf; -// uint8_t byte = 0; - -// while (true) { -// size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); -// if (ec) { -// RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); -// break; -// } - -// if (ret > 0) { -// sliding_buf.push_back(byte); -// RCLCPP_INFO(this->get_logger(), "Recv: 0x%02X", byte); -// if (sliding_buf.size() > 1024) { -// sliding_buf.clear(); -// } -// } -// } -// } - std::vector AGV_PRO::read_serial_response( const std::vector& expected_header, size_t payload_size, double timeout_sec) { - std::vector buffer; + std::vector sliding_buf; uint8_t byte = 0; rclcpp::Time start_time = this->now(); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec); - while (rclcpp::ok()) { + while ((this->now() - start_time) < timeout) { boost::asio::mutable_buffers_1 buf(&byte, 1); boost::system::error_code ec; - size_t len = serial_port_->read_some(boost::asio::buffer(&byte,1), ec); + size_t n = serial_port_->read_some(buf, ec); if (ec) { RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); - break; + return {}; } - if (len > 0) { - buffer.push_back(byte); - //**log */ - RCLCPP_INFO(this->get_logger(), "recv byte: 0x%02X", byte); - std::string buf_str; - for (auto b : buffer) { - char tmp[5]; - snprintf(tmp, sizeof(tmp), "%02X ", b); - buf_str += tmp; + if (n == 1) { + sliding_buf.push_back(byte); + if (sliding_buf.size() > expected_header.size()) { + sliding_buf.erase(sliding_buf.begin()); } - std::string header_str; - for (auto b : expected_header) { - char tmp[5]; - snprintf(tmp, sizeof(tmp), "%02X ", b); - header_str += tmp; - } - RCLCPP_INFO(this->get_logger(), "buffer: %s", buf_str.c_str()); - RCLCPP_INFO(this->get_logger(), "expected_header: %s", header_str.c_str()); - //**log */ - if (buffer.size() >= expected_header.size()) { - bool match = true; - for (size_t i=0;iget_logger(), "No data received within timeout!"); - } else { - RCLCPP_INFO(this->get_logger(), "Final buffer size=%zu", buffer.size()); + + if (sliding_buf != expected_header) { + RCLCPP_WARN(this->get_logger(), "Timeout waiting for header"); + return {}; } - return buffer; + + size_t remain_len = payload_size + 2; + std::vector remain_buf(remain_len); + size_t total_read = 0; + + while (total_read < remain_len && (this->now() - start_time) < timeout) { + boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + total_read += n; + } + + if (total_read != remain_len) { + RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload"); + return {}; + } + + std::vector full_buf = expected_header; + full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end()); + + return full_buf; } bool AGV_PRO::is_power_on(){ @@ -163,7 +141,7 @@ bool AGV_PRO::is_power_on(){ auto status_query_frame = build_serial_frame(0x10, {}); send_serial_frame(status_query_frame,true); - //rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command + rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x10}; auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds @@ -519,6 +497,8 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) this->clearSerialBuffer(fd); this->disableDTR_RTS(fd); + rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time + RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully"); RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str()); From 79fa72a8b02b71bb2386d23f3b8fe2c4f5778ab8 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Thu, 11 Sep 2025 17:27:00 +0800 Subject: [PATCH 4/7] fix(agv_pro_description): Fix missing imu_link --- agv_pro_description/urdf/agv_pro.urdf | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/agv_pro_description/urdf/agv_pro.urdf b/agv_pro_description/urdf/agv_pro.urdf index b91f3e0..9824f10 100755 --- a/agv_pro_description/urdf/agv_pro.urdf +++ b/agv_pro_description/urdf/agv_pro.urdf @@ -221,4 +221,12 @@ + + + + + + + + \ No newline at end of file From c6dbf7084eac6debea6173051a1134d69bb8b2af Mon Sep 17 00:00:00 2001 From: X-lanni Date: Fri, 19 Sep 2025 13:45:59 +0800 Subject: [PATCH 5/7] feat(agv_pro_autocharge): Added automatic recharge function code --- agv_pro_autocharge/README.md | 134 ++++ .../agv_pro_autocharge/__init__.py | 10 + .../combined_auto_recharger.py | 612 ++++++++++++++++++ .../agv_pro_autocharge/serial_can_parser.py | 173 +++++ .../config/charger_position.json | 6 + .../config/nav_goal_params.yaml | 3 + agv_pro_autocharge/package.xml | 30 + .../resource/agv_pro_autocharge | 1 + agv_pro_autocharge/setup.cfg | 4 + agv_pro_autocharge/setup.py | 30 + 10 files changed, 1003 insertions(+) create mode 100755 agv_pro_autocharge/README.md create mode 100755 agv_pro_autocharge/agv_pro_autocharge/__init__.py create mode 100755 agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py create mode 100755 agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py create mode 100755 agv_pro_autocharge/config/charger_position.json create mode 100755 agv_pro_autocharge/config/nav_goal_params.yaml create mode 100755 agv_pro_autocharge/package.xml create mode 100755 agv_pro_autocharge/resource/agv_pro_autocharge create mode 100755 agv_pro_autocharge/setup.cfg create mode 100755 agv_pro_autocharge/setup.py diff --git a/agv_pro_autocharge/README.md b/agv_pro_autocharge/README.md new file mode 100755 index 0000000..dbcb04e --- /dev/null +++ b/agv_pro_autocharge/README.md @@ -0,0 +1,134 @@ +# AGV AutoCharge ROS2 Package + +这是一个为ROS2 Humble设计的AGV自动充电系统软件包。 + +## 功能特性 + +- 监听充电桩位置更新 +- 持续发布可视化标记 +- 键盘交互启动导航功能 +- 导航成功后启动串口控制 +- 支持充电状态检测和控制 + +## 安装依赖 + +确保您的系统已安装以下依赖： + +```bash +# ROS2 Humble基础包 +sudo apt install ros-humble-rclpy +sudo apt install ros-humble-geometry-msgs +sudo apt install ros-humble-std-msgs +sudo apt install ros-humble-nav-msgs +sudo apt install ros-humble-visualization-msgs +sudo apt install ros-humble-nav2-simple-commander + +# Python依赖 +pip3 install pyserial +``` + +## 编译安装 + +```bash +# 进入ROS2工作空间 +cd /path/to/your/ros2_ws/src + +# 复制软件包到工作空间 +cp -r agv_autocharge_ros2 . + +# 编译软件包 +cd .. +colcon build --packages-select agv_autocharge_ros2 + +# 加载环境变量 +source install/setup.bash +``` + +## 使用方法 + +### 启动节点 + +```bash +# 启动自动充电控制器节点 +ros2 run agv_autocharge_ros2 combined_auto_recharger +``` + +### 节点功能 + +- 监听 `/charger_position_update` 话题，接收充电桩位置更新 +- 发布 `/goal_marker` 话题，在RViz中显示充电桩位置标记 +- 发布 `/cmd_vel` 话题，控制机器人运动 +- 按键 `q` 启动导航到充电桩 +- 导航成功后自动启动串口控制 + +### 配置文件 + +充电桩位置配置文件位于： +``` +config/charger_position.json +``` + +文件格式： +```json +{ + "p_x": 1.329015495451769, + "p_y": 0.31961151635354823, + "orien_z": 0.4981823289472456, + "orien_w": 0.8670722963655905 +} +``` + +### 话题接口 + +#### 订阅话题 +- `/charger_position_update` (geometry_msgs/PoseStamped): 充电桩位置更新 + +#### 发布话题 +- `/goal_marker` (visualization_msgs/MarkerArray): 充电桩位置可视化标记 +- `/cmd_vel` (geometry_msgs/Twist): 机器人运动控制 +- `/chassis_security` (std_msgs/Int8): 底盘安全控制 + +### 串口配置 + +默认串口配置： +- 端口: `/dev/ttyCH341USB0` +- 波特率: 9600 +- 超时: 1秒 + +可以根据需要修改代码中的串口参数。 + +## 操作说明 + +1. 启动节点后，系统会自动加载充电桩位置配置 +2. 系统会定期发布充电桩标记到RViz进行可视化 +3. 按下键盘上的 `q` 键启动导航到充电桩 +4. 导航成功后，系统会自动启动串口控制功能 +5. 串口控制会根据接收到的数据控制机器人运动 +6. 按 `Ctrl+C` 退出程序 + +## 故障排除 + +### 常见问题 + +1. **串口无法打开** + - 检查串口设备是否连接 + - 确认串口权限设置 + - 验证串口设备名称 + +2. **导航失败** + - 确认Nav2导航系统正常运行 + - 检查充电桩位置配置是否正确 + - 验证地图和定位系统状态 + +3. **RViz中看不到标记** + - 确认RViz已订阅 `/goal_marker` 话题 + - 检查MarkerArray显示设置 + - 验证坐标系设置是否为'map' + +## 许可证 + +MIT License + +## 维护者 + +请联系维护者获取技术支持。 diff --git a/agv_pro_autocharge/agv_pro_autocharge/__init__.py b/agv_pro_autocharge/agv_pro_autocharge/__init__.py new file mode 100755 index 0000000..caaf2a7 --- /dev/null +++ b/agv_pro_autocharge/agv_pro_autocharge/__init__.py @@ -0,0 +1,10 @@ +""" +AGV AutoCharge ROS2 Package + +This package provides automatic charging functionality for AGV robots using ROS2 Humble. +It includes position management, visualization, navigation, and serial control features. +""" + +__version__ = '1.0.0' +__author__ = 'Your Name' +__email__ = 'your-email@example.com' diff --git a/agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py b/agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py new file mode 100755 index 0000000..81d8fe5 --- /dev/null +++ b/agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py @@ -0,0 +1,612 @@ +#!/usr/bin/env python3 +# coding=utf-8 + +""" +合并的自动充电控制器 - 结合位置管理、可视化和导航功能 +- 监听充电桩位置更新 +- 持续发布可视化标记 +- 按键'q'启动导航功能 +- 导航成功后启动串口控制 +""" + +# 引用ros库 +import rclpy +from rclpy.node import Node +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from rclpy.duration import Duration + +# 用到的变量定义 +from std_msgs.msg import Bool +from std_msgs.msg import Int8 +from std_msgs.msg import UInt8 +from std_msgs.msg import Float32 + +# 用于记录充电桩位置、发布导航点 +from geometry_msgs.msg import PoseStamped, Twist + +# rviz可视化相关 +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + +# 里程计话题相关 +from nav_msgs.msg import Odometry + +# 键盘控制相关 +import sys +import select +import termios +import tty + +# 延迟相关 +import time +import threading + +# 读写充电桩位置文件 +import json +import math +import yaml +import os + +# 导入串口解析模块 +from .serial_can_parser import SerialCANParser + +# 存放充电桩位置的文件位置 - 参考原始auto_recharger.py的路径设置方式 +def find_config_files(): + """查找配置文件路径""" + # 首先尝试几个可能的位置 + possible_paths = [ + # 开发环境路径 + '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config', + # 你的工作空间路径 + '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config', + # 当前包的相对路径 + os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'config'), + # 安装路径 + '/home/elephant/agv_pro_ros2/install/agv_pro_autocharge/share/agv_pro_autocharge/config' + ] + + for config_dir in possible_paths: + yaml_path = os.path.join(config_dir, 'nav_goal_params.yaml') + json_path = os.path.join(config_dir, 'charger_position.json') + + print(f"Checking config directory: {config_dir}") + if os.path.exists(yaml_path) and os.path.exists(json_path): + print(f"Found config files in: {config_dir}") + return yaml_path, json_path + + # 如果都找不到，直接报错 + print("ERROR: Could not find config files in any of the following locations:") + for path in possible_paths: + print(f" - {path}") + print("Please ensure the config files exist in one of these directories.") + + # 返回第一个路径作为默认值，但文件可能不存在 + return os.path.join(possible_paths[0], 'nav_goal_params.yaml'), os.path.join(possible_paths[0], 'charger_position.json') + +# 获取配置文件路径 +yaml_file, json_file = find_config_files() + +# print_and_fixRetract相关，用于打印带颜色的信息 +RESET = '\033[0m' +RED = '\033[1;31m' +GREEN = '\033[1;32m' +YELLOW= '\033[1;33m' +BLUE = '\033[1;34m' +PURPLE= '\033[1;35m' +CYAN = '\033[1;36m' + +# 圆周率 +PI = 3.1415926535897 + +if os.name == 'nt': + import msvcrt +else: + import termios + import tty + +settings = None +if os.name != 'nt' and sys.stdin.isatty(): + settings = list(termios.tcgetattr(sys.stdin)) + +def get_key(settings): + if os.name == 'nt': + return msvcrt.getch().decode('utf-8') + else: + if sys.stdin.isatty(): + tty.setraw(sys.stdin.fileno()) + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) + if rlist: + key = sys.stdin.read(1) + else: + key = '' + if sys.stdin.isatty() and settings: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + +def print_and_fixRetract(str): + global settings + '''键盘控制会导致回调函数内使用print()出现自动缩进的问题，此函数可以解决该现象''' + if sys.stdin.isatty() and settings: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + print(str) + +class CombinedAutoRecharger(Node): + def __init__(self): + + # 创建节点 + super().__init__("combined_auto_recharger") + + print_and_fixRetract('Combined Auto Recharger Node Started!') + + # 导航状态标记 + self.navigation_active = False + + # 串口控制相关 + self.parser = None + self.serial_control_active = False + self.navigation_requested = False # 添加导航请求标志 + + # 创建导航器 + self.navigator = BasicNavigator() + + # 加载充电桩位置信息 + self.load_charger_position() + # 加载导航参数 + self.load_nav_goal_params() + + # 创建发布者 + self.robot_security_off_pub = self.create_publisher(Int8, '/chassis_security', 10) + self.Charger_marker_pub = self.create_publisher(MarkerArray, '/goal_marker', 10) + self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) + + # 创建订阅者 - 只订阅充电桩位置更新 + self.Charger_Position_Update_sub = self.create_subscription( + PoseStamped, "/charger_position_update", + self.Position_Update_callback, 10) + + # 创建定时器，定期发布充电桩标记（每2秒发布一次） + self.marker_timer = self.create_timer(2.0, self.timer_callback) + + # 创建导航检查定时器（每0.5秒检查一次导航请求） + self.navigation_timer = self.create_timer(0.5, self.check_navigation_request) + + # 发布初始充电桩位置标记 + self.update_charger_visualization() + + print_and_fixRetract('Combined auto recharger node initialized successfully!') + print_and_fixRetract(f'{GREEN}Press "q" to start navigation, Ctrl+C to exit{RESET}') + + def load_nav_goal_params(self): + """加载导航目标参数（前方距离和角度）""" + print_and_fixRetract(f"Attempting to load nav goal params from: {yaml_file}") + print_and_fixRetract(f"File exists? {os.path.exists(yaml_file)}") + + try: + with open(yaml_file, 'r', encoding='utf-8') as f: + params = yaml.safe_load(f) + + print_and_fixRetract(f"Raw params from file: {params}") + + self.forward_distance = float(params.get('forward_distance', 1.0)) + self.yaw_offset_deg = float(params.get('yaw_offset_deg', 0.0)) + + print_and_fixRetract(f"Successfully loaded nav goal params: forward_distance={self.forward_distance}, yaw_offset_deg={self.yaw_offset_deg}") + + except FileNotFoundError: + print_and_fixRetract(f"{RED}Nav goal params file not found: {yaml_file}{RESET}") + print_and_fixRetract(f"{RED}Please create the configuration file with the required parameters{RESET}") + # 使用默认值 + self.forward_distance = 1.0 + self.yaw_offset_deg = 0.0 + except Exception as e: + print_and_fixRetract(f"{RED}Failed to load nav goal params: {e}{RESET}") + self.forward_distance = 1.0 + self.yaw_offset_deg = 0.0 + + def timer_callback(self): + '''定时器回调函数，定期发布充电桩标记''' + # 始终发布当前JSON文件中的位置信息 + if hasattr(self, 'json_data') and self.json_data: + self.Pub_Charger_marker( + self.json_data['p_x'], + self.json_data['p_y'], + self.json_data['orien_z'], + self.json_data['orien_w'] + ) + + def check_navigation_request(self): + '''检查是否有导航请求''' + if self.navigation_requested and not self.navigation_active: + self.navigation_requested = False + self.navigation_active = True + print_and_fixRetract(f"{BLUE}Processing navigation request...{RESET}") + + # 在ROS2线程中执行导航 + result = self.execute_navigation_internal() + + if result: + print_and_fixRetract(f"{GREEN}Navigation successful! Starting serial control...{RESET}") + # 在ROS2线程中启动串口控制 + self.start_serial_control_async() + else: + print_and_fixRetract(f"{RED}Navigation failed{RESET}") + self.navigation_active = False + + def request_navigation(self): + '''请求开始导航''' + if not self.navigation_active: + self.navigation_requested = True + print_and_fixRetract(f"{BLUE}Navigation request queued...{RESET}") + else: + print_and_fixRetract(f"{YELLOW}Navigation already in progress{RESET}") + + def load_charger_position(self): + '''加载充电桩位置信息''' + try: + with open(json_file, 'r', encoding='utf-8') as fp: + self.json_data = json.load(fp) + print_and_fixRetract(f"Loaded charger position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}") + except FileNotFoundError: + print_and_fixRetract(f"{RED}Charger position file {json_file} not found{RESET}") + print_and_fixRetract(f"{RED}Please create the configuration file with default charger position{RESET}") + # 使用默认位置 + self.json_data = { + 'p_x': 0.0, + 'p_y': 0.0, + 'orien_z': 0.0, + 'orien_w': 1.0 + } + except Exception as e: + print_and_fixRetract(f"Error loading charger position: {e}") + self.json_data = { + 'p_x': 0.0, + 'p_y': 0.0, + 'orien_z': 0.0, + 'orien_w': 1.0 + } + + def save_charger_position(self): + '''保存充电桩位置信息到JSON文件''' + try: + with open(json_file, 'w', encoding='utf-8') as fp: + json.dump(self.json_data, fp, ensure_ascii=False, indent=2) + print_and_fixRetract(f"{GREEN}Charger position saved to {json_file}{RESET}") + except Exception as e: + print_and_fixRetract(f"{RED}Error saving charger position: {e}{RESET}") + + def Pub_Charger_Position(self): + '''更新充电桩位置信息并保存到JSON文件''' + # 发布充电桩位置的可视化 + self.Pub_Charger_marker( + self.json_data['p_x'], + self.json_data['p_y'], + self.json_data['orien_z'], + self.json_data['orien_w']) + + # 保存当前充电桩位置到JSON文件 + position_data = { + 'p_x': self.json_data['p_x'], + 'p_y': self.json_data['p_y'], + 'orien_z': self.json_data['orien_z'], + 'orien_w': self.json_data['orien_w'] + } + + self.json_data = position_data + self.save_charger_position() + print_and_fixRetract(f"Position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}") + + def Pub_Charger_marker(self, p_x, p_y, o_z, o_w): + '''发布目标点可视化话题''' + + markerArray = MarkerArray() + + # 获取当前时间戳 + current_time = self.get_clock().now().to_msg() + + marker_shape = Marker() # 创建marker对象 + marker_shape.id = 0 # 必须赋值id + marker_shape.header.frame_id = 'map' # 以哪一个TF坐标为原点 + marker_shape.header.stamp = current_time # 添加时间戳 + marker_shape.type = Marker.ARROW # TEXT_VIEW_FACING #一直面向屏幕的字符格式 + marker_shape.action = Marker.ADD # 添加marker + marker_shape.scale.x = 0.5 # marker大小 + marker_shape.scale.y = 0.05 # marker大小 + marker_shape.scale.z = 0.05 # marker大小，对于字符只有z起作用 + marker_shape.pose.position.x = p_x # 字符位置 + marker_shape.pose.position.y = p_y # 字符位置 + marker_shape.pose.position.z = 0.1 # msg.position.z #字符位置 + marker_shape.pose.orientation.z = o_z # 字符位置 + marker_shape.pose.orientation.w = o_w # 字符位置 + marker_shape.color.r = 1.0 # 字符颜色R(红色)通道 + marker_shape.color.g = 0.0 # 字符颜色G(绿色)通道 + marker_shape.color.b = 0.0 # 字符颜色B(蓝色)通道 + marker_shape.color.a = 1.0 # 字符透明度 + markerArray.markers.append(marker_shape) # 添加元素进数组 + + marker_string = Marker() # 创建marker对象 + marker_string.id = 1 # 必须赋值id + marker_string.header.frame_id = 'map' # 以哪一个TF坐标为原点 + marker_string.header.stamp = current_time # 添加时间戳 + marker_string.type = Marker.TEXT_VIEW_FACING # 一直面向屏幕的字符格式 + marker_string.action = Marker.ADD # 添加marker + marker_string.scale.x = 0.5 # marker大小 + marker_string.scale.y = 0.5 # marker大小 + marker_string.scale.z = 0.5 # marker大小，对于字符只有z起作用 + marker_string.color.a = 1.0 # 字符透明度 + marker_string.color.r = 1.0 # 字符颜色R(红色)通道 + marker_string.color.g = 0.0 # 字符颜色G(绿色)通道 + marker_string.color.b = 0.0 # 字符颜色B(蓝色)通道 + marker_string.pose.position.x = p_x # 字符位置 + marker_string.pose.position.y = p_y # 字符位置 + marker_string.pose.position.z = 0.1 # msg.position.z #字符位置 + marker_string.pose.orientation.z = o_z # 字符位置 + marker_string.pose.orientation.w = o_w # 字符位置 + marker_string.text = 'Charger' # 字符内容 + markerArray.markers.append(marker_string) # 添加元素进数组 + self.Charger_marker_pub.publish(markerArray) # 发布markerArray，rviz订阅并进行可视化 + + def Position_Update_callback(self, topic): + '''更新json文件中的充电桩位置''' + position_dic = {'p_x': 0, 'p_y': 0, 'orien_z': 0, 'orien_w': 0} + position_dic['p_x'] = topic.pose.position.x + position_dic['p_y'] = topic.pose.position.y + position_dic['orien_z'] = topic.pose.orientation.z + position_dic['orien_w'] = topic.pose.orientation.w + + # 保存最新的充电桩位置到json文件 + self.json_data = position_dic + self.save_charger_position() + print_and_fixRetract("New charging pile position saved.") + + # 位置更新后立即发布一次新的标记，然后继续定时发布 + self.update_charger_visualization() + print_and_fixRetract(f"{GREEN}Charger position updated and will be published continuously{RESET}") + + def update_charger_visualization(self): + '''更新充电桩可视化标记''' + if hasattr(self, 'json_data'): + self.Pub_Charger_marker( + self.json_data['p_x'], + self.json_data['p_y'], + self.json_data['orien_z'], + self.json_data['orien_w'] + ) + + def execute_navigation(self): + """外部调用的导航接口""" + self.request_navigation() + return True # 返回True表示请求已提交 + + def execute_navigation_internal(self): + """内部执行导航任务""" + print_and_fixRetract(f"{BLUE}Starting navigation...{RESET}") + # 从JSON文件读取充电桩位置 + try: + with open(json_file, 'r', encoding='utf-8') as f: + charger_data = json.load(f) + px = charger_data['p_x'] + py = charger_data['p_y'] + # 充电桩姿态四元数转欧拉角 + orien_z = charger_data['orien_z'] + orien_w = charger_data['orien_w'] + yaw = 2 * math.atan2(orien_z, orien_w) # 只考虑z/w分量 + except Exception as e: + print_and_fixRetract(f"{RED}Failed to read charger position file: {e}{RESET}") + self.navigation_active = False + return False + + # 计算目标点位置 + x_offset = self.forward_distance * math.cos(yaw) + y_offset = self.forward_distance * math.sin(yaw) + goal_x = px + x_offset + goal_y = py + y_offset + + # 计算目标点姿态（z轴顺时针yaw_offset_deg） + goal_yaw = yaw - math.radians(self.yaw_offset_deg) + goal_qz = math.sin(goal_yaw / 2) + goal_qw = math.cos(goal_yaw / 2) + + print_and_fixRetract(f"Nav goal: x={goal_x:.3f}, y={goal_y:.3f}, yaw={math.degrees(goal_yaw):.1f}°") + goal_pose = self.create_pose(goal_x, goal_y, goal_qz, goal_qw) + + # 执行导航 + print_and_fixRetract(f"{BLUE}Executing navigation...{RESET}") + result1 = self.nav_through_pose([goal_pose], verbose=False) + print_and_fixRetract(f"Navigation result: {result1}") + self.navigation_active = False + return result1 + + def create_pose(self, x, y, z, w): + """创建单个目标点的位姿信息""" + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = self.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.z = z + pose.pose.orientation.w = w + return pose + + def nav_through_pose(self, goal_poses, verbose: bool = False) -> bool: + """执行多点导航任务""" + # 开始执行多点导航任务 + self.navigator.goThroughPoses(goal_poses) + + # 等待导航任务完成，监控导航状态 + while not self.navigator.isTaskComplete(): + feedback = self.navigator.getFeedback() + if feedback and verbose: + remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9 + print_and_fixRetract(f"预计到达时间: {remaining:.0f} 秒") + + # 根据导航结果返回相应状态 + result = self.navigator.getResult() + if result == TaskResult.SUCCEEDED: + print_and_fixRetract(f'{GREEN}Navigation successful!{RESET}') + return True + elif result == TaskResult.CANCELED: + print_and_fixRetract(f'{YELLOW}Navigation canceled!{RESET}') + elif result == TaskResult.FAILED: + print_and_fixRetract(f'{RED}Navigation failed!{RESET}') + else: + print_and_fixRetract(f'{RED}Invalid navigation result!{RESET}') + return False + + def start_serial_control_async(self): + """异步启动串口控制功能""" + def serial_control_thread(): + self.start_serial_control() + + # 在新线程中启动串口控制，避免阻塞ROS2主线程 + serial_thread = threading.Thread(target=serial_control_thread, daemon=True) + serial_thread.start() + + def start_serial_control(self): + """启动串口控制功能""" + print_and_fixRetract(f"{BLUE}Starting serial control...{RESET}") + try: + self.parser = SerialCANParser('/dev/agvpro_ec130', 9600, 1) + self.parser.open_serial() # 打开usb串口 + + # 发送AT 命令从透传模式进入AT指令模式 + self.parser.send_at_commands(["AT+CG", "AT+AT"]) + + self.serial_control_active = True + print_and_fixRetract(f'{GREEN}Serial control started successfully{RESET}') + + while self.serial_control_active: + # 开始读取数据 + x_speed, z_speed, which_mode, infrared_bits = self.parser.read_serial_data() + + if infrared_bits[7] == 0: # 无障碍物 + if which_mode == 0x01: # 正常模式 + # 直接发布ROS2 Twist消息 + twist_msg = Twist() + twist_msg.linear.x = float(x_speed) + twist_msg.linear.y = 0.0 + twist_msg.angular.z = float(z_speed) + self.cmd_vel_pub.publish(twist_msg) + print_and_fixRetract(f'Normal mode - Speed: x={x_speed}, z={z_speed}') + + elif which_mode == 0xBB: # 测压区 + # 停止运动 + stop_msg = Twist() + self.cmd_vel_pub.publish(stop_msg) + print_and_fixRetract(f'{YELLOW}Pressure zone - Stop movement{RESET}') + + elif which_mode == 0xAA: # 充电区 + # 停止运动 + stop_msg = Twist() + self.cmd_vel_pub.publish(stop_msg) + print_and_fixRetract(f'{GREEN}Charging zone - Stop movement{RESET}') + break # 充电完成后退出 + + elif which_mode == 0xCF: # 急停模式 + emergency_stop_msg = Twist() # 所有速度都为0 + self.cmd_vel_pub.publish(emergency_stop_msg) + print_and_fixRetract(f'{RED}Emergency stop mode - Immediate stop{RESET}') + break + + else: # 检测到障碍物 + obstacle_stop_msg = Twist() # 所有速度都为0 + self.cmd_vel_pub.publish(obstacle_stop_msg) + print_and_fixRetract(f'{RED}Obstacle detected - Stop movement{RESET}') + break + + except KeyboardInterrupt: + print_and_fixRetract("Serial control interrupted by user") + # 发布停止消息 + emergency_stop = Twist() + self.cmd_vel_pub.publish(emergency_stop) + except Exception as e: + print_and_fixRetract(f"{RED}Serial control error: {e}{RESET}") + # 发布停止消息 + emergency_stop = Twist() + self.cmd_vel_pub.publish(emergency_stop) + finally: + if self.parser: + self.parser.close_serial() + print_and_fixRetract("Serial control stopped") + + def stop_serial_control(self): + """停止串口控制功能""" + self.serial_control_active = False + + def get_charger_info(self): + '''获取充电桩位置信息''' + if hasattr(self, 'json_data'): + return { + 'position': { + 'x': self.json_data['p_x'], + 'y': self.json_data['p_y'] + }, + 'orientation': { + 'z': self.json_data['orien_z'], + 'w': self.json_data['orien_w'] + } + } + return None + + +def main(args=None): + '''主函数''' + rclpy.init(args=args) + + combined_recharger = None + try: + combined_recharger = CombinedAutoRecharger() + + print_and_fixRetract("Combined auto recharger node is running...") + print_and_fixRetract("Node functions:") + print_and_fixRetract("- Listening for charger position updates on /charger_position_update") + print_and_fixRetract("- Publishing visualization markers on /goal_marker every 2 seconds") + print_and_fixRetract("- Press 'q' to start navigation to charger position") + print_and_fixRetract("- Navigation success will trigger serial control") + + # 启动ROS2事件循环线程 + def ros2_spin(): + try: + rclpy.spin(combined_recharger) + except Exception as spin_error: + print_and_fixRetract(f"ROS2 spin error: {spin_error}") + + ros2_thread = threading.Thread(target=ros2_spin, daemon=True) + ros2_thread.start() + + # 键盘监听循环 + print_and_fixRetract(f"{GREEN}Press 'q' to start navigation, Ctrl+C to exit{RESET}") + print_and_fixRetract("Waiting for keyboard input...") + + while True: + try: + key = get_key(settings) + if key: + print_and_fixRetract(f"Key pressed: {repr(key)}") # 调试信息 + if key.lower() == 'q': + print_and_fixRetract(f"{BLUE}Navigation command received!{RESET}") + + # 请求导航任务（异步执行） + combined_recharger.execute_navigation() + + elif key == '\x03': # Ctrl+C + break + + time.sleep(0.1) # 避免过度占用CPU + except KeyboardInterrupt: + break + except Exception as e: + print_and_fixRetract(f"Keyboard input error: {e}") + time.sleep(0.5) + + except KeyboardInterrupt: + print_and_fixRetract("\nShutting down combined auto recharger node...") + finally: + if combined_recharger: + combined_recharger.stop_serial_control() + combined_recharger.destroy_node() + rclpy.shutdown() + print_and_fixRetract("Program exited safely") + +if __name__ == '__main__': + main() + diff --git a/agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py b/agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py new file mode 100755 index 0000000..4277f58 --- /dev/null +++ b/agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 +#coding=UTF-8 +import serial +import time + +class SerialCANParser: + def __init__(self, serial_port='/dev/ttyCH341USB0', baudrate=9600, timeout=1): + self.serial_port = serial_port # 串口名称 + self.baudrate = baudrate # 波特率 + self.timeout = timeout # 超时设置 + self.ser = None # 串口对象 + self.buffer = bytearray() # 存储当前读取的字节 + self.max_retries = 3 # 最大重试次数 + # 存储实时数据 + self.x_speed = 0.0 + self.z_speed = 0.0 + self.infrared_bits = [] + + def open_serial(self): + """打开串口""" + try: + self.ser = serial.Serial(self.serial_port, self.baudrate, timeout=self.timeout) + print(f"串口 {self.serial_port} 已打开，波特率：{self.baudrate}") + except Exception as e: + print(f"打开串口失败: {e}") + + def close_serial(self): + """关闭串口""" + if self.ser and self.ser.is_open: + self.ser.close() + print("串口已关闭。") + else: + print("串口未打开或已关闭。") + + def can_id_check(self, date): + high_byte, low_byte = date[0:2] + # 高字节左移 3 位 + can_id = (high_byte << 3) + # 低字节右移 5 位 + can_id |= (low_byte >> 5) + return can_id + + def parse_can_data(self, data): + """解析8字节CAN数据帧""" + if len(data) != 8: + print("数据帧长度不正确") + return None + + # 解析 X、Y 和 Z 速度 + x_speed_raw = ((data[0] << 8) | data[1]) # X速度的原始数据 + z_speed_raw = ((data[4] << 8) | data[5]) # Z速度的原始数据 + + # 将原始数据转换为浮动数值，并考虑正负 + if x_speed_raw & 0x8000: # 如果最高位为1，表示负数 + x_speed_raw = -((65536 - x_speed_raw) & 0xFFFF) # 补码转换为负数 + if z_speed_raw & 0x8000: # 如果最高位为1，表示负数 + z_speed_raw = -((65536 - z_speed_raw) & 0xFFFF) # 补码转换为负数 + + # 转换单位为 m/s 和 rad/s + self.x_speed = x_speed_raw / 1000.0 # X速度单位为 m/s + self.y_speed = 0 # Y速度为0 + self.z_speed = z_speed_raw / 1000.0 # Z速度单位为 rad/s + self.which_mode = data[2] + self.infrared = data[6] # 红外数据 + self.raw_current = data[7] # 电流数据 + + if self.raw_current > 32767: # 无符号数大于 32767 表示负值（因为最大值是 65535） + # 转换为负数 + self.actual_current = -(65536 - self.raw_current) * 30.0 + else: + # 正数直接转换 + self.actual_current = self.raw_current * 30.0 + + # 处理红外数据 + self.infrared_bits = [(self.infrared >> (7 - i)) & 0x01 for i in range(8)] + + # 打印或处理数据 + print(f"X Speed: {self.x_speed:.3f}, Y Speed: {self.y_speed}, Z Speed: {self.z_speed:.3f}, " + f"Actual Current: {self.actual_current:.3f} mA, Infrared: {self.infrared}") + + # 打印或处理红外位信息 + print(f"L_A: {self.infrared_bits[2]}, L_B: {self.infrared_bits[3]}, R_B: {self.infrared_bits[4]}, " + f"R_A: {self.infrared_bits[5]}, infrared_flag : {self.infrared_bits[6]}, " + f"Charging flag: {self.infrared_bits[7]}") + + def read_serial_data(self): + """读取串口数据并解析""" + while True: # 修改为简单的无限循环，由上层控制退出 + if self.ser.in_waiting > 0: + byte = self.ser.read(1) # 读取一个字节 + if len(self.buffer) < 2: + self.buffer.extend(byte) + if len(self.buffer) == 2: + # 如果帧头为 0x41 0x54，表示为AT帧头，则开始接收数据 + if self.buffer[0] != 0x41 or self.buffer[1] != 0x54: + # 如果不是有效的帧头，则清空缓冲区并跳到下次循环 + self.buffer.clear() + continue # 继续等待下一个字节 + else: + self.buffer.extend(byte) + # print("缓冲区内容:", ' '.join(f'{b:02x}' for b in self.buffer)) # debug + # 如果缓冲区字节长度大于等于 17 字节（数据帧长度） + if len(self.buffer) >= 17: + # print("Received Frame (Hex):", ' '.join(f'{byte:02x}' for byte in self.buffer)) # debug + # 解析帧头、CAN帧ID、格式、类型和数据 + # at_frame_header = self.buffer[0:2] # AT帧头 + can_frame_id = self.can_id_check(self.buffer[2:4]) # CAN标准帧ID + # can_frame_format = self.buffer[4] # CAN帧格式（0,标准帧;1,扩展帧） + # can_frame_type = self.buffer[5] # CAN帧类型（0,数据帧;1,远程帧） + data_length = self.buffer[6] # 数据长度 + data = self.buffer[7:15] # 数据帧 + # print(f"帧ID: 0x{can_frame_id:X}") # debug + if can_frame_id == 0x182 and data_length == 0x08: # 根据can帧id进行判断 + # 如果帧ID为0x182,校验通过,进行数据赋值 + self.parse_can_data(data) + # 清空缓冲区，准备下一帧数据 + self.buffer.clear() + return self.x_speed, self.z_speed, self.which_mode, self.infrared_bits # 返回解析后的数据 + else: + # 清空缓冲区，准备下一帧数据 + self.buffer.clear() + + def read_serial_response(self): + """读取串口响应数据，直到接收到 '\r\n' 或超时""" + response = bytearray() # 使用 bytearray 来存储原始字节流 + while True: + if self.ser.in_waiting > 0: + byte = self.ser.read(1) + response += byte + # 检查是否已接收到完整响应 + if b'\r\n' in response: + break + # 超时机制，防止死循环 + if len(response) > 100: + break + return bytes(response) # 返回原始字节流（bytes） + + def send_at_commands(self, commands): + """发送 AT 命令并等待响应""" + for command in commands: + retries = 0 + while retries < self.max_retries: + self.ser.write(command.encode() + b'\r\n') + print(f"发送命令: {command}") + # 等待响应并读取数据 + response = self.read_serial_response() + # 检查响应是否包含 "OK" + if b"OK" in response: + print(f"收到响应: {response}") + break # 如果收到 OK，退出重试循环 + else: + retries += 1 + print(f"未收到预期的响应，收到: {response}") + if retries == self.max_retries: + print(f"重试 {self.max_retries} 次后仍未收到有效响应，请检查设备。") + break + + def start(self): + """开始读取和处理数据""" + self.open_serial() + try: + # 发送AT 命令从透传模式进入AT指令模式 + self.send_at_commands(["AT+CG", "AT+AT"]) + # 开始读取数据 + self.read_serial_data() + except KeyboardInterrupt: + print("手动中止程序。") + finally: + self.close_serial() + +if __name__ == '__main__': + parser = SerialCANParser('/dev/ttyCH341USB0', 9600, 1) + parser.start() diff --git a/agv_pro_autocharge/config/charger_position.json b/agv_pro_autocharge/config/charger_position.json new file mode 100755 index 0000000..7b738ef --- /dev/null +++ b/agv_pro_autocharge/config/charger_position.json @@ -0,0 +1,6 @@ +{ + "p_x": -0.04373347759246826, + "p_y": -4.024151802062988, + "orien_z": 0.09378381732290733, + "orien_w": 0.9955925851513477 +} \ No newline at end of file diff --git a/agv_pro_autocharge/config/nav_goal_params.yaml b/agv_pro_autocharge/config/nav_goal_params.yaml new file mode 100755 index 0000000..e9339be --- /dev/null +++ b/agv_pro_autocharge/config/nav_goal_params.yaml @@ -0,0 +1,3 @@ +# 导航参数配置 +forward_distance: 1 # 距离充电桩前方1米 +yaw_offset_deg: 10.0 # 顺时针旋转10度 diff --git a/agv_pro_autocharge/package.xml b/agv_pro_autocharge/package.xml new file mode 100755 index 0000000..cc05bd0 --- /dev/null +++ b/agv_pro_autocharge/package.xml @@ -0,0 +1,30 @@ + + + + agv_pro_autocharge + 1.0.0 + AGV automatic charging system for ROS2 Humble + Your Name + MIT + + + ament_python + + + rclpy + geometry_msgs + std_msgs + nav_msgs + visualization_msgs + nav2_simple_commander + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/agv_pro_autocharge/resource/agv_pro_autocharge b/agv_pro_autocharge/resource/agv_pro_autocharge new file mode 100755 index 0000000..0138642 --- /dev/null +++ b/agv_pro_autocharge/resource/agv_pro_autocharge @@ -0,0 +1 @@ +agv_pro_autocharge diff --git a/agv_pro_autocharge/setup.cfg b/agv_pro_autocharge/setup.cfg new file mode 100755 index 0000000..9b4840c --- /dev/null +++ b/agv_pro_autocharge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/agv_pro_autocharge +[install] +install_scripts=$base/lib/agv_pro_autocharge diff --git a/agv_pro_autocharge/setup.py b/agv_pro_autocharge/setup.py new file mode 100755 index 0000000..0dd0998 --- /dev/null +++ b/agv_pro_autocharge/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup, find_packages +import os +from glob import glob + +package_name = 'agv_pro_autocharge' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # Include config files + (os.path.join('share', package_name, 'config'), glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Your Name', + maintainer_email='your-email@example.com', + description='AGV automatic charging system for ROS2 Humble', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'combined_auto_recharger = agv_pro_autocharge.combined_auto_recharger:main', + ], + }, +) From 049d3995bab8b94ab24ea6761bbec0e6e1dfcced Mon Sep 17 00:00:00 2001 From: X-lanni Date: Tue, 23 Sep 2025 10:11:30 +0800 Subject: [PATCH 6/7] feat(navigation2): add RViz charger tool --- navigation2/nav2_rviz_plugins/CMakeLists.txt | 2 + .../nav2_rviz_plugins/charger_tool.hpp | 69 +++++++++++++++ .../nav2_rviz_plugins/plugins_description.xml | 6 ++ .../nav2_rviz_plugins/src/charger_tool.cpp | 87 +++++++++++++++++++ 4 files changed, 164 insertions(+) create mode 100644 navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp create mode 100644 navigation2/nav2_rviz_plugins/src/charger_tool.cpp diff --git a/navigation2/nav2_rviz_plugins/CMakeLists.txt b/navigation2/nav2_rviz_plugins/CMakeLists.txt index 39bdbab..44ac329 100644 --- a/navigation2/nav2_rviz_plugins/CMakeLists.txt +++ b/navigation2/nav2_rviz_plugins/CMakeLists.txt @@ -36,6 +36,7 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/goal_pose_updater.hpp include/nav2_rviz_plugins/goal_common.hpp include/nav2_rviz_plugins/goal_tool.hpp + include/nav2_rviz_plugins/charger_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp @@ -49,6 +50,7 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/goal_tool.cpp + src/charger_tool.cpp src/nav2_panel.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp diff --git a/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp b/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp new file mode 100644 index 0000000..673bf3e --- /dev/null +++ b/navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_ +#define NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_ + +#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" +#include + +#include "rviz_default_plugins/tools/pose/pose_tool.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_common +{ + +class DisplayContext; + +namespace properties +{ +class StringProperty; +class QosProfileProperty; +} // namespace properties +} // namespace rviz_common + +namespace nav2_rviz_plugins +{ + +class RVIZ_DEFAULT_PLUGINS_PUBLIC ChargerTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + ChargerTool(); + ~ChargerTool() override; + + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Clock::SharedPtr clock_; + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::QosProfileProperty * qos_profile_property_; + + rclcpp::QoS qos_profile_; +}; + +} // namespace nav2_rviz_plugins + +#endif // NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_ diff --git a/navigation2/nav2_rviz_plugins/plugins_description.xml b/navigation2/nav2_rviz_plugins/plugins_description.xml index 197a9a7..0eb7522 100644 --- a/navigation2/nav2_rviz_plugins/plugins_description.xml +++ b/navigation2/nav2_rviz_plugins/plugins_description.xml @@ -6,6 +6,12 @@ A tool used to specify the navigation goal pose. + + A tool used to specify the charging stations goal pose. + + diff --git a/navigation2/nav2_rviz_plugins/src/charger_tool.cpp b/navigation2/nav2_rviz_plugins/src/charger_tool.cpp new file mode 100644 index 0000000..e2e685d --- /dev/null +++ b/navigation2/nav2_rviz_plugins/src/charger_tool.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_rviz_plugins/charger_tool.hpp" + +#include +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/load_resource.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/qos_profile_property.hpp" + +namespace nav2_rviz_plugins +{ + +ChargerTool::ChargerTool() +: rviz_default_plugins::tools::PoseTool(), qos_profile_(5) +{ + shortcut_key_ = 'c'; + topic_property_ = new rviz_common::properties::StringProperty( + "Topic", "charger_position_update", + "The topic on which to publish goals.", + getPropertyContainer(), SLOT(updateTopic()), this); + + qos_profile_property_ = new rviz_common::properties::QosProfileProperty( + topic_property_, qos_profile_); +} + +ChargerTool::~ChargerTool() +{ +} + +void ChargerTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("Charger Update"); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png")); + updateTopic(); +} + +void ChargerTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = + context_->getRosNodeAbstraction().lock()->get_raw_node(); + // TODO(anhosi, wjwwood): replace with abstraction for publishers once available + publisher_ = raw_node-> + template create_publisher( + topic_property_->getStdString(), qos_profile_); + clock_ = raw_node->get_clock(); +} + +void +ChargerTool::onPoseSet(double x, double y, double theta) +{ + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = clock_->now(); + goal.header.frame_id = fixed_frame; + + goal.pose.position.x = x; + goal.pose.position.y = y; + goal.pose.position.z = 0.0; + + goal.pose.orientation = orientationAroundZAxis(theta); + + logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame); + + publisher_->publish(goal); +} + +} // namespace nav2_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ChargerTool, rviz_common::Tool) From cf17bb899b66a7f73c8ba076d10397432097f254 Mon Sep 17 00:00:00 2001 From: X-lanni Date: Sun, 28 Sep 2025 19:18:59 +0800 Subject: [PATCH 7/7] feat(rtabmap_ros): Add demo configurations, launch files, and navigation parameters for agvpro --- .../rtabmap_demos/config/rtabmap_rgbd.rviz | 756 ++++++++++++++++ .../config/rtabmap_rgbd_scan.rviz | 819 ++++++++++++++++++ .../rtabmap_demos/config/rtabmap_scan.rviz | 683 +++++++++++++++ .../launch/agvpro/agvpro_mapping.launch.py | 109 +++ .../launch/agvpro/agvpro_rgbd.launch.py | 132 +++ .../launch/agvpro/agvpro_rgbd_scan.launch.py | 161 ++++ .../launch/agvpro/agvpro_scan.launch.py | 133 +++ .../params/agvpro_rgbd_nav2_params.yaml | 301 +++++++ .../params/agvpro_rgbd_scan_nav2_params.yaml | 301 +++++++ .../params/agvpro_scan_nav2_params.yaml | 295 +++++++ 10 files changed, 3690 insertions(+) create mode 100755 rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz create mode 100755 rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz create mode 100755 rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz create mode 100644 rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py create mode 100644 rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py create mode 100644 rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py create mode 100644 rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py create mode 100644 rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml create mode 100644 rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml create mode 100644 rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml diff --git a/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz new file mode 100755 index 0000000..5cf819a --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd.rviz @@ -0,0 +1,756 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 418 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_front_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_rear_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_front_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_rear_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_color_frame: + Value: true + camera_color_optical_frame: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true + camera_ir_frame: + Value: true + camera_ir_optical_frame: + Value: true + camera_link: + Value: true + imu_link: + Value: true + laser_link: + Value: true + left_front_wheel_link: + Value: true + left_rear_wheel_link: + Value: true + map: + Value: true + odom: + Value: true + right_front_wheel_link: + Value: true + right_rear_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + camera_link: + camera_depth_frame: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + {} + camera_ir_frame: + camera_ir_optical_frame: + {} + imu_link: + {} + laser_link: + {} + left_front_wheel_link: + {} + left_rear_wheel_link: + {} + right_front_wheel_link: + {} + right_rear_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 227 + Min Color: 0; 0; 0 + Min Intensity: 17 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.036771878600120544 + Min Value: -0.163384810090065 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/ground + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.3959618806838989 + Min Value: 0.05118126422166824 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/obstacles + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 5.277319431304932 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a20000035afc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000760000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001df000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000222000001750000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff00000001000002440000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005920000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1850 + X: 70 + Y: 27 diff --git a/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz new file mode 100755 index 0000000..bfe4e7f --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/config/rtabmap_rgbd_scan.rviz @@ -0,0 +1,819 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 417 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_front_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_rear_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_front_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_rear_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_color_frame: + Value: true + camera_color_optical_frame: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true + camera_ir_frame: + Value: true + camera_ir_optical_frame: + Value: true + camera_link: + Value: true + imu_link: + Value: true + laser_link: + Value: true + left_front_wheel_link: + Value: true + left_rear_wheel_link: + Value: true + map: + Value: true + odom: + Value: true + right_front_wheel_link: + Value: true + right_rear_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + camera_link: + camera_depth_frame: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + {} + camera_ir_frame: + camera_ir_optical_frame: + {} + imu_link: + {} + laser_link: + {} + left_front_wheel_link: + {} + left_rear_wheel_link: + {} + right_front_wheel_link: + {} + right_rear_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 228 + Min Color: 0; 0; 0 + Min Intensity: 7 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.048584431409835815 + Min Value: -0.123008593916893 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/ground + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.39776936173439026 + Min Value: 0.05130460113286972 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/obstacles + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rtabmap_rviz_plugins/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Landmark: 0; 128; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mapGraph + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.3704118728637695 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a20000035afc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000760000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000221000001760000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff00000001000002fb0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005920000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1850 + X: 70 + Y: 27 diff --git a/rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz b/rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz new file mode 100755 index 0000000..6fb706d --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/config/rtabmap_scan.rviz @@ -0,0 +1,683 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 370 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_front_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_rear_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_front_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_rear_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_color_frame: + Value: true + camera_color_optical_frame: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true + camera_ir_frame: + Value: true + camera_ir_optical_frame: + Value: true + camera_link: + Value: true + imu_link: + Value: true + laser_link: + Value: true + left_front_wheel_link: + Value: true + left_rear_wheel_link: + Value: true + map: + Value: true + odom: + Value: true + right_front_wheel_link: + Value: true + right_rear_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + camera_link: + camera_depth_frame: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + {} + camera_ir_frame: + camera_ir_optical_frame: + {} + imu_link: + {} + laser_link: + {} + left_front_wheel_link: + {} + left_rear_wheel_link: + {} + right_front_wheel_link: + {} + right_rear_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 230 + Min Color: 0; 0; 0 + Min Intensity: 8 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5707999467849731 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 171.56951904296875 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 932 + Hide Left Dock: false + Hide Right Dock: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a200000306fc020000000afb0000001200530065006c0065006300740069006f006e000000003d000000760000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000001f2000001510000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff00000001000002fb0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006090000003efc0100000002fb0000000800540069006d0065010000000000000609000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004610000030600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1545 + X: 208 + Y: 43 diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py new file mode 100644 index 0000000..a493ab0 --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_mapping.launch.py @@ -0,0 +1,109 @@ +# Requirements: +# Compile agv_pro_base and agv_pro_bringup packages +# +# Example: +# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py +# $ ros2 launch orbbec_camera gemini2.launch.py +# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'odom_frame_id':'odom', + 'odom_tf_linear_variance':0.001, + 'odom_tf_angular_variance':0.001, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's internal parameters should be strings + 'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans + 'RGBD/ProximityBySpace': 'true', # Local loop closure detection (using estimated position) with locations in WM + 'RGBD/ProximityByTime': 'false', # Local loop closure detection with locations in STM + 'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together. + 'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP + 'Vis/MinInliers': '12', # 3D visual words minimum inliers to accept loop closure + 'RGBD/OptimizeFromGraphEnd': 'false', # Optimize graph from initial node so /map -> /odom transform will be generated + 'RGBD/OptimizeMaxError': '4', # Reject any loop closure causing large errors (>3x link's covariance) in the map + 'Reg/Force3DoF': 'true', # 2D SLAM + 'Grid/FromDepth': 'false', # Create 2D occupancy grid from laser scan + 'Mem/STMSize': '30', # increased to 30 to avoid adding too many loop closures on just seen locations + 'RGBD/LocalRadius': '5', # limit length of proximity detections + 'Icp/CorrespondenceRatio': '0.2', # minimum scan overlap to accept loop closure + 'Icp/PM': 'false', + 'Icp/PointToPlane': 'false', + 'Icp/MaxCorrespondenceDistance': '0.15', + 'Icp/VoxelSize': '0.05' + } + + remappings=[ + ('rgb/image', '/camera/color/image_raw'), + ('depth/image', '/camera/depth/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('scan', '/scan')] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=False), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + { + # 'rgb_image_transport':'compressed', + # 'depth_image_transport':'compressedDepth', + 'approx_sync_max_interval': 0.1}], + remappings=remappings), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py new file mode 100644 index 0000000..805265f --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd.launch.py @@ -0,0 +1,132 @@ +# Requirements: +# Compile agv_pro_base and agv_pro_bringup packages +# +# Example: +# Bringup agvpro: +# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py +# +# Bringup orbbec gemini2 camera: +# $ ros2 launch orbbec_camera gemini2.launch.py +# +# SLAM: +# $ ros2 launch rtabmap_demos agvpro_rgbd_demo.launch.py +# +# Teleop: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'use_sim_time':use_sim_time, + 'subscribe_depth':True, + 'use_action_for_goal':True, + 'Reg/Force3DoF':'true', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/RangeMax':'3', + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + + remappings=[ + ('rgb/image', '/camera/color/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('depth/image', '/camera/depth/image_raw')] + + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_rgbd_nav2_params.yaml'] + ) + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + # Nodes to launch + + # Navigation2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'false'), + ('params_file', nav2_params_file) + ] + ), + + # RViz + IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ), + + # SLAM mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters], + remappings=remappings), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/depth/camera_info'), + ('cloud', '/camera/depth_registered/points')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/depth_registered/points'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), + ]) diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py new file mode 100644 index 0000000..63e8719 --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_rgbd_scan.launch.py @@ -0,0 +1,161 @@ +# Requirements: +# Compile agv_pro_base and agv_pro_bringup packages +# +# Example: +# Bringup agvpro: +# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py +# +# Bringup orbbec gemini2 camera: +# $ ros2 launch orbbec_camera gemini2.launch.py +# +# SLAM: +# $ ros2 launch rtabmap_demos agvpro_rgbd_scan.launch.py +# +# Teleop: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard + +import os +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'use_sim_time':use_sim_time, + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'use_action_for_goal':True, + 'approx_sync':True, + 'sync_queue_size': 10, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', + 'RGBD/NeighborLinkRefining':'True', + 'Grid/RayTracing':'true', # Fill empty space + 'Grid/3D':'false', # Use 2D occupancy + 'Grid/RangeMax':'3', + 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles + 'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map + 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored + 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + + remappings=[ + ('rgb/image', '/camera/color/image_raw'), + ('depth/image', '/camera/depth/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('scan', '/scan')] + + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_rgbd_scan_nav2_params.yaml'] + ) + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'rtabmap_viz', default_value='false', + description='Launch RTAB-Map UI (optional).'), + + DeclareLaunchArgument( + 'rviz', default_value='true', + description='Launch RVIZ (optional).'), + + DeclareLaunchArgument( + 'rviz_cfg', default_value=os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'rtabmap_rgbd_scan.rviz'), + description='Configuration path of rviz2.'), + + # Nodes to launch + # Navigation2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'false'), + ('params_file', nav2_params_file) + ] + ), + + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[parameters, + { + 'use_sim_time':use_sim_time, + 'approx_sync_max_interval': 0.1}], + remappings=remappings), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + # Visualization: + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + + # Obstacle detection with the camera for nav2 local costmap. + # First, we need to convert depth image to a point cloud. + # Second, we segment the floor from the obstacles. + Node( + package='rtabmap_util', executable='point_cloud_xyz', output='screen', + parameters=[{'decimation': 2, + 'max_depth': 3.0, + 'voxel_size': 0.02}], + remappings=[('depth/image', '/camera/depth/image_raw'), + ('depth/camera_info', '/camera/depth/camera_info'), + ('cloud', '/camera/depth_registered/points')]), + Node( + package='rtabmap_util', executable='obstacles_detection', output='screen', + parameters=[parameters], + remappings=[('cloud', '/camera/depth_registered/points'), + ('obstacles', '/camera/obstacles'), + ('ground', '/camera/ground')]), + ]) diff --git a/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py new file mode 100644 index 0000000..599c991 --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/launch/agvpro/agvpro_scan.launch.py @@ -0,0 +1,133 @@ +# Requirements: +# Compile agv_pro_base and agv_pro_bringup packages +# +# Example: +# Bringup agvpro: +# $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py +# +# SLAM: +# $ ros2 launch rtabmap_demos agvpro_scan_demo.launch.py +# +# Teleop: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction,IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def launch_setup(context, *args, **kwargs): + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'True' or localization == 'true' + icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' + + parameters={ + 'frame_id':'base_footprint', + 'use_sim_time':use_sim_time, + 'subscribe_depth':False, + 'subscribe_rgb':False, + 'subscribe_scan':True, + 'approx_sync':True, + 'use_action_for_goal':True, + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', + 'RGBD/NeighborLinkRefining':'True', + 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself + 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) + } + arguments = [] + if localization: + parameters['Mem/IncrementalMemory'] = 'False' + parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + remappings=[ + ('scan', '/scan')] + if icp_odometry: + remappings.append(('odom', 'icp_odom')) + # modified nav2 params to use icp_odom instead odom frame + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_scan_nav2_params.yaml'] + ) + else: + # original nav2 params + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('agv_pro_navigation2'), 'param', 'agvpro.yaml'] + ) + + # Directories + pkg_nav2_bringup = get_package_share_directory( + 'nav2_bringup') + + # Paths + nav2_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'navigation_launch.py']) + rviz_launch = PathJoinSubstitution( + [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) + + return [ + # Nodes to launch + + # Navigation2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch]), + launch_arguments=[ + ('use_sim_time', 'false'), + ('params_file', nav2_params_file) + ] + ), + + # RViz + IncludeLaunchDescription( + PythonLaunchDescriptionSource([rviz_launch]) + ), + + # ICP odometry (optional) + Node( + condition=IfCondition(LaunchConfiguration('icp_odometry')), + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[parameters, + {'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom'}], + remappings=remappings), + + # SLAM: + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=arguments), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters], + remappings=remappings), + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'icp_odometry', default_value='false', + description='Launch ICP odometry on top of wheel odometry.'), + + OpaqueFunction(function=launch_setup) + ]) diff --git a/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml new file mode 100644 index 0000000..5a07ca9 --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_nav2_params.yaml @@ -0,0 +1,301 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan ground obstacles + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: True + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml new file mode 100644 index 0000000..7efa6b5 --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/params/agvpro_rgbd_scan_nav2_params.yaml @@ -0,0 +1,301 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.15 + movement_time_allowance: 8.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan ground obstacles + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: True + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml b/rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml new file mode 100644 index 0000000..10b4d00 --- /dev/null +++ b/rtabmap_ros/rtabmap_demos/params/agvpro_scan_nav2_params.yaml @@ -0,0 +1,295 @@ +# Modified to use icp_odom frame +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.15 + movement_time_allowance: 8.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 0.5 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 2.25 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -2.25 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 40 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 23.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 18.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: icp_odom + robot_base_frame: base_footprint + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: True + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: icp_odom + robot_base_frame: base_footprint + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0