From 4eefeb4b61d0d258c53857b61fba323057f02a9f Mon Sep 17 00:00:00 2001 From: X-lanni Date: Sat, 7 Jun 2025 16:41:43 +0800 Subject: [PATCH] Update driver interface and bringup config for AGV Pro --- .../include/agv_pro_base/agv_pro_driver.h | 1 + agv_pro_base/src/agv_pro_ros.cpp | 185 ++++++++++++------ .../launch/agv_pro_bringup.launch.py | 6 +- agv_pro_bringup/package.xml | 1 + agv_pro_description/CMakeLists.txt | 2 +- 5 files changed, 132 insertions(+), 63 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 8502eee..f5e3ff1 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 @@ -27,6 +27,7 @@ public: ~AGV_PRO(); private: void Control(); + void set_auto_report(); bool readData(); void publisherOdom(double dt); void publisherVoltage(); diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index 38819ad..49ac508 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -16,64 +16,6 @@ std::array odom_twist_covariance = { 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9} }; -AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) -{ - this->declare_parameter("port_name","/dev/ttyS0"); - this->declare_parameter("odometry.frame_id", "odom"); - this->declare_parameter("odometry.child_frame_id", "base_footprint"); - this->declare_parameter("imu.frame_id", "imu_link"); - this->declare_parameter("namespace", ""); - - this->get_parameter_or("port_name",device_name_,std::string("")); - this->get_parameter_or("odometry.frame_id",frame_id_of_odometry_,std::string("odom")); - this->get_parameter_or("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint")); - this->get_parameter_or("imu.frame_id",frame_id_of_imu_,std::string("imu_link")); - this->get_parameter_or("namespace",name_space_,std::string("")); - - if (name_space_ != "") { - frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_; - child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_; - frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_; - } - - odomBroadcaster = std::make_unique(this); - pub_imu = this->create_publisher("imu", 20); - pub_odom = this->create_publisher("odom", 50); - pub_voltage = create_publisher("voltage", 10); - cmd_sub = this->create_subscription( - "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1)); - - drivers::serial_driver::SerialPortConfig config( - 115200, - 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()); - } - catch (const std::exception &ex){ - RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what()); - return; - } - - AGV_PRO::Control();//Loop through data collection and publish the topic - -} - -AGV_PRO::~AGV_PRO() -{ - -} - uint16_t crc16_ibm(const uint8_t* data, size_t length) { uint16_t crc = 0xFFFF; for (size_t i = 0; i < length; ++i) { @@ -88,6 +30,40 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) { return crc; } +void AGV_PRO::set_auto_report(){ + + std::array buf = { + 0xFE, 0xFE, 0x23, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00 + }; + + uint16_t crc = crc16_ibm(buf.data(), buf.size()); + buf[11] = crc & 0xff; + buf[12] = (crc >> 8) & 0xff; + + std::vector data_vec(buf.begin(), buf.end()); + + auto port = serial_driver_->port(); + + try + { + port->send(data_vec); + // size_t bytes_transmit_size = port->send(data_vec); + // std::stringstream ss; + // for (auto b : data_vec) { + // ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2) + // << static_cast(b) << " "; + // } + // RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str()); + } + catch(const std::exception &ex) + { + RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what()); + } + +} + void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { linearX = std::clamp(msg->linear.x, -1.5, 1.5); @@ -270,9 +246,100 @@ void AGV_PRO::Control() if (true == readData()) { publisherOdom(dt); - //RCLCPP_INFO(this->get_logger(), "dt:%f", dt); + RCLCPP_INFO(this->get_logger(), "dt:%f", dt); publisherVoltage(); } lastTime = currentTime; } +} + +AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) +{ + this->declare_parameter("port_name","/dev/agvpro_controller"); + this->declare_parameter("odometry.frame_id", "odom"); + this->declare_parameter("odometry.child_frame_id", "base_footprint"); + this->declare_parameter("imu.frame_id", "imu_link"); + this->declare_parameter("namespace", ""); + + this->get_parameter_or("port_name",device_name_,std::string("/dev/agvpro_controller")); + this->get_parameter_or("odometry.frame_id",frame_id_of_odometry_,std::string("odom")); + this->get_parameter_or("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint")); + this->get_parameter_or("imu.frame_id",frame_id_of_imu_,std::string("imu_link")); + this->get_parameter_or("namespace",name_space_,std::string("")); + + if (name_space_ != "") { + frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_; + child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_; + frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_; + } + + odomBroadcaster = std::make_unique(this); + pub_imu = this->create_publisher("imu", 20); + pub_odom = this->create_publisher("odom", 50); + pub_voltage = create_publisher("voltage", 10); + cmd_sub = this->create_subscription( + "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1)); + + 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()); + + AGV_PRO::set_auto_report(); + } + catch (const std::exception &ex){ + RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what()); + return; + } + + AGV_PRO::Control();//Loop through data collection and publish the topic + +} + +AGV_PRO::~AGV_PRO() +{ + std::array buf = { + 0xFE, 0xFE, 0x22, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00 + }; + + uint16_t crc = crc16_ibm(buf.data(), buf.size()); + buf[11] = crc & 0xff; + buf[12] = (crc >> 8) & 0xff; + + std::vector data_vec(buf.begin(), buf.end()); + + auto port = serial_driver_->port(); + + try + { + port->send(data_vec); + // size_t bytes_transmit_size = port->send(data_vec); + // std::stringstream ss; + // for (auto b : data_vec) { + // ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2) + // << static_cast(b) << " "; + // } + // RCLCPP_INFO(this->get_logger(), "Sent %ld bytes: [%s]", bytes_transmit_size, ss.str().c_str()); + } + 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"); } \ No newline at end of file diff --git a/agv_pro_bringup/launch/agv_pro_bringup.launch.py b/agv_pro_bringup/launch/agv_pro_bringup.launch.py index 5fabb20..d202c7e 100644 --- a/agv_pro_bringup/launch/agv_pro_bringup.launch.py +++ b/agv_pro_bringup/launch/agv_pro_bringup.launch.py @@ -8,7 +8,7 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - port_name_arg = LaunchConfiguration('port_name',default='ttyS0') + port_name_arg = LaunchConfiguration('port_name',default='/dev/agvpro_controller') namespace = LaunchConfiguration('namespace', default='') urdf_file = os.path.join( @@ -24,7 +24,7 @@ def generate_launch_description(): DeclareLaunchArgument( 'port_name', default_value=port_name_arg, - description='port name, e.g. ttyS0'), + description='port name, e.g. ttyACM0'), Node( package='agv_pro_base', @@ -53,6 +53,6 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( get_package_share_directory('lslidar_driver'),'launch'), - 'lslidar_launch.py']) + '/lslidar_launch.py']) ) ]) diff --git a/agv_pro_bringup/package.xml b/agv_pro_bringup/package.xml index 864d457..2c642f8 100644 --- a/agv_pro_bringup/package.xml +++ b/agv_pro_bringup/package.xml @@ -9,6 +9,7 @@ ament_cmake robot_state_publisher + joint_state_publisher rviz2 agv_pro_description agv_pro_base diff --git a/agv_pro_description/CMakeLists.txt b/agv_pro_description/CMakeLists.txt index 8369145..287c342 100755 --- a/agv_pro_description/CMakeLists.txt +++ b/agv_pro_description/CMakeLists.txt @@ -14,7 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -install(DIRECTORY meshes urdf +install(DIRECTORY meshes urdf launch DESTINATION share/${PROJECT_NAME} )