From b754d612aaac01d257206333088ed9d4043f7f82 Mon Sep 17 00:00:00 2001 From: taorye Date: Fri, 11 Aug 2023 09:56:27 +0800 Subject: [PATCH] update ros1 for a010 --- sipeed_tof_ms_a010_ros/ros1/CMakeLists.txt | 29 +- sipeed_tof_ms_a010_ros/ros1/package.xml | 56 +++- .../ros1/src/frame_handle.cc | 65 +++-- .../ros1/src/frame_struct.h | 29 +- sipeed_tof_ms_a010_ros/ros1/src/main.cc | 260 ----------------- sipeed_tof_ms_a010_ros/ros1/src/msa010.hpp | 227 +++++++++++++++ .../ros1/src/msa010_auto_test.cc | 102 +++++++ .../ros1/src/msa010_cin_test.cc | 40 +++ sipeed_tof_ms_a010_ros/ros1/src/serial.cc | 182 ------------ sipeed_tof_ms_a010_ros/ros1/src/serial.hh | 28 -- .../ros1/src/sipeed_tof_ms_a010_node.cc | 266 ++++++++++++++++++ sipeed_tof_ms_a010_ros/ros1/src/test.cc | 83 ------ 12 files changed, 749 insertions(+), 618 deletions(-) delete mode 100644 sipeed_tof_ms_a010_ros/ros1/src/main.cc create mode 100644 sipeed_tof_ms_a010_ros/ros1/src/msa010.hpp create mode 100644 sipeed_tof_ms_a010_ros/ros1/src/msa010_auto_test.cc create mode 100644 sipeed_tof_ms_a010_ros/ros1/src/msa010_cin_test.cc delete mode 100644 sipeed_tof_ms_a010_ros/ros1/src/serial.cc delete mode 100644 sipeed_tof_ms_a010_ros/ros1/src/serial.hh create mode 100644 sipeed_tof_ms_a010_ros/ros1/src/sipeed_tof_ms_a010_node.cc delete mode 100644 sipeed_tof_ms_a010_ros/ros1/src/test.cc diff --git a/sipeed_tof_ms_a010_ros/ros1/CMakeLists.txt b/sipeed_tof_ms_a010_ros/ros1/CMakeLists.txt index cb31b6b..4055b43 100644 --- a/sipeed_tof_ms_a010_ros/ros1/CMakeLists.txt +++ b/sipeed_tof_ms_a010_ros/ros1/CMakeLists.txt @@ -12,11 +12,11 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs - message_generation ) -find_package(OpenCV REQUIRED) + ## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +find_package(Boost REQUIRED COMPONENTS system) +find_package(OpenCV REQUIRED) ## Uncomment this if the package has a setup.py. This macro ensures @@ -106,8 +106,8 @@ find_package(OpenCV REQUIRED) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES sipeed_tof_cpp - CATKIN_DEPENDS message_runtime cv_bridge roscpp rospy std_msgs +# LIBRARIES sipeed_tof_ms_a010 + CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) @@ -120,11 +120,13 @@ catkin_package( include_directories( # include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/sipeed_tof_cpp.cpp +# src/${PROJECT_NAME}/sipeed_tof_ms_a010.cpp # ) ## Add cmake target dependencies of the library @@ -135,21 +137,20 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/sipeed_tof_cpp_node.cpp) -add_executable(a010_publisher src/main.cc src/cJSON.c src/serial.cc src/frame_handle.cc) -# target_link_libraries(a010_publisher) +add_executable(${PROJECT_NAME}_node src/sipeed_tof_ms_a010_node.cc src/cJSON.c src/frame_handle.cc) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") +set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above -add_dependencies(a010_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -target_link_libraries(a010_publisher +target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ) @@ -169,7 +170,7 @@ target_link_libraries(a010_publisher ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS a010_publisher +install(TARGETS ${PROJECT_NAME}_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) @@ -200,7 +201,7 @@ install(TARGETS a010_publisher ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_sipeed_tof_cpp.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_sipeed_tof_ms_a010.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/sipeed_tof_ms_a010_ros/ros1/package.xml b/sipeed_tof_ms_a010_ros/ros1/package.xml index e3a9caf..bebf160 100644 --- a/sipeed_tof_ms_a010_ros/ros1/package.xml +++ b/sipeed_tof_ms_a010_ros/ros1/package.xml @@ -2,29 +2,67 @@ sipeed_tof_ms_a010 0.0.0 - ros1 support for MS-a010 - taorye - TODO: License declaration + The sipeed_tof_ms_a010 package + + + + taorye + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin - cv_bridge roscpp rospy std_msgs - message_generation - cv_bridge roscpp rospy std_msgs - cv_bridge roscpp rospy std_msgs - message_runtime + - \ No newline at end of file + diff --git a/sipeed_tof_ms_a010_ros/ros1/src/frame_handle.cc b/sipeed_tof_ms_a010_ros/ros1/src/frame_handle.cc index 1b641b4..1b308e1 100644 --- a/sipeed_tof_ms_a010_ros/ros1/src/frame_handle.cc +++ b/sipeed_tof_ms_a010_ros/ros1/src/frame_handle.cc @@ -2,12 +2,13 @@ #include // #include +// using namespace std; +#include #include #include #include "frame_struct.h" - -frame_t *handle_process(std::string s) { +frame_t *handle_process(const std::string &s) { static std::vector vecChar; static const uint8_t sflag_l = FRAME_BEGIN_FLAG & 0xff; static const uint8_t sflag_h = (FRAME_BEGIN_FLAG >> 8) & 0xff; @@ -17,7 +18,9 @@ frame_t *handle_process(std::string s) { frame_t *pf = NULL; std::vector::iterator it; + // cout << "vecChar before: " << vecChar.size() << endl; vecChar.insert(vecChar.end(), s.cbegin(), s.cend()); + // cout << "vecChar after: " << vecChar.size() << endl; if (vecChar.size() < 2) { // cerr << "data is not enough!" << endl; @@ -25,28 +28,25 @@ frame_t *handle_process(std::string s) { } __find_header: - it = vecChar.begin(); - do { - /* find sflag_h from [1:] first and next in [it+1:] */ - it = find(it + 1, vecChar.end(), sflag_h); - /* sflag_h not found */ + for (it = vecChar.begin(); (*(it) != sflag_l) || (*(it + 1) != sflag_h);) { + /* find sflag_l from [1:] first and next in [it+1:] */ + it = find(it + 1, vecChar.end(), sflag_l); + /* sflag_l not found */ if (it == vecChar.end()) { - /* keep last element which may be sflag_l */ - std::vector(vecChar.end() - 1, vecChar.end()).swap(vecChar); + /* clear all data and wait next data */ + vecChar.resize(0); // cerr << "frame head not found! wait more data." << endl; goto __finished; } - /* sflag_h found, *(it-1) always valid */ - } while (*(it - 1) != sflag_l); - /* we got *it==sflag_h and *(it-1)==sflag_l */ + } - if (it - 1 != vecChar.begin()) { - std::vector(it - 1, vecChar.end()).swap(vecChar); + if (it != vecChar.begin()) { + std::vector(it, vecChar.end()).swap(vecChar); // cerr << "frame move to first!" << endl; } if (vecChar.size() < sizeof(frame_t)) { - // cerr << "frame head data is not enough now! wait more data." << endl; + // cerr << "frame head data not enough now! wait more data." << endl; goto __finished; } @@ -55,28 +55,37 @@ frame_t *handle_process(std::string s) { /* max frame payload size */ if (frame_payload_len > 100 * 100) { + // cerr << "frame head data invalid for large frame_payload_len." << endl; + vecChar.pop_back(); + vecChar.pop_back(); goto __find_header; } - if (vecChar.begin() + FRAME_HEAD_SIZE + frame_payload_len + - FRAME_CHECKSUM_SIZE + FRAME_END_SIZE + 1 > - vecChar.end()) { + if (vecChar.size() < FRAME_HEAD_SIZE + frame_payload_len + + FRAME_CHECKSUM_SIZE + FRAME_END_SIZE) { // cerr << "expected frame payload length: " << frame_payload_len << endl; - // cerr << "frame payload data is not enough now! wait more data." << endl; + // cerr << "frame payload data not enough now! wait more data." << endl; goto __finished; } { - static uint8_t check_sum = 0; - check_sum = 0; - for (uint32_t i = 0; i < FRAME_HEAD_SIZE + frame_payload_len; i++) { - check_sum += ((uint8_t *)pf)[i]; - } + uint8_t check_sum = std::accumulate( + vecChar.begin(), vecChar.begin() + FRAME_HEAD_SIZE + frame_payload_len, + (uint8_t)0); + if (check_sum != ((uint8_t *)pf)[FRAME_HEAD_SIZE + frame_payload_len] || eflag != ((uint8_t *)pf)[FRAME_HEAD_SIZE + frame_payload_len + FRAME_CHECKSUM_SIZE]) { - // cerr << "frame checksum or tail invalid! one more time." << endl; - std::vector(it, vecChar.end()).swap(vecChar); + // cerr << "src\tchecksum\ttail" << endl; + // cerr << "data\t" + // << *(vecChar.begin() + FRAME_HEAD_SIZE + frame_payload_len) << + // '\t' + // << *(vecChar.begin() + FRAME_HEAD_SIZE + frame_payload_len + + // FRAME_CHECKSUM_SIZE) + // << endl; + // cerr << "data\t" << check_sum << '\t' << eflag << endl; + vecChar.pop_back(); + vecChar.pop_back(); goto __find_header; } } @@ -84,8 +93,8 @@ frame_t *handle_process(std::string s) { pf = (frame_t *)malloc(sizeof(frame_t) + frame_payload_len); memcpy(pf, &vecChar[0], sizeof(frame_t) + frame_payload_len); - std::vector(it + FRAME_HEAD_SIZE + frame_payload_len + - FRAME_CHECKSUM_SIZE + FRAME_END_SIZE - 1, + std::vector(vecChar.begin() + FRAME_HEAD_SIZE + frame_payload_len + + FRAME_CHECKSUM_SIZE + FRAME_END_SIZE, vecChar.end()) .swap(vecChar); return pf; diff --git a/sipeed_tof_ms_a010_ros/ros1/src/frame_struct.h b/sipeed_tof_ms_a010_ros/ros1/src/frame_struct.h index 603aa39..97ce867 100644 --- a/sipeed_tof_ms_a010_ros/ros1/src/frame_struct.h +++ b/sipeed_tof_ms_a010_ros/ros1/src/frame_struct.h @@ -30,26 +30,27 @@ typedef struct { uint8_t isp_version; uint8_t reserved3; // fixed to 0xff } __attribute__((packed)) frame_head_t; +static_assert(FRAME_HEAD_SIZE == sizeof(frame_head_t), "err"); typedef struct { frame_head_t frame_head; uint8_t payload[]; } __attribute__((packed)) frame_t; -// typedef struct { -// uint8_t cali_mode; // 0:Normal, 1:Fisheye -// uint32_t fx; // fixpoint: u14p18 -// uint32_t fy; // fixpoint: u14p18 -// uint32_t u0; // fixpoint: u14p18 -// uint32_t v0; // fixpoint: u14p18 -// uint32_t k1; // fixpoint: s5p27 -// uint32_t k2; // fixpoint: s5p27 -// uint32_t k3; // fixpoint: s5p27 -// uint32_t k4_p1; // fixpoint: s5p27, normal mode is k4, fisheye mode is p1 -// uint32_t k5_p2; // fixpoint: s5p27, normal mode is k5 or unused, fisheye -// // mode is p2 -// uint32_t skew; // fixpoint: s8p24 -// } __attribute__((packed)) LensCoeff_t; +typedef struct { + uint8_t cali_mode; // 0:Normal, 1:Fisheye + uint32_t fx; // fixpoint: u14p18 + uint32_t fy; // fixpoint: u14p18 + uint32_t u0; // fixpoint: u14p18 + uint32_t v0; // fixpoint: u14p18 + uint32_t k1; // fixpoint: s5p27 + uint32_t k2; // fixpoint: s5p27 + uint32_t k3; // fixpoint: s5p27 + uint32_t k4_p1; // fixpoint: s5p27, normal mode is k4, fisheye mode is p1 + uint32_t k5_p2; // fixpoint: s5p27, normal mode is k5 or unused, fisheye + // mode is p2 + uint32_t skew; // fixpoint: s8p24 +} __attribute__((packed)) LensCoeff_t; #ifdef __cplusplus } diff --git a/sipeed_tof_ms_a010_ros/ros1/src/main.cc b/sipeed_tof_ms_a010_ros/ros1/src/main.cc deleted file mode 100644 index e3d693e..0000000 --- a/sipeed_tof_ms_a010_ros/ros1/src/main.cc +++ /dev/null @@ -1,260 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "cJSON.h" -#include "frame_struct.h" -#include "serial.hh" - -extern frame_t *handle_process(std::string s); -extern const uint8_t color_lut_jet[][3]; - -using namespace std::chrono_literals; - -#define ser (*pser) -static Serial *pser; -static float uvf_parms[4]; -static ros::NodeHandle *localNode; -static ros::Publisher publisher_depth; -static ros::Publisher publisher_pointcloud; - -static void InitSipeedTOF_MSA010_Publisher(ros::NodeHandle *n) { - localNode = n; - std::string s; - localNode->param("device", s, "/dev/ttyUSB0"); - // this->declare_parameter("device", "/dev/ttyUSB0"); - // rclcpp::Parameter device_param = this->get_parameter("device"); - // s = device_param.as_string(); - std::cout << "use device: " << s << std::endl; - pser = new Serial(s); - - ser << "AT+DISP=1\r"; - do { - ser >> s; - } while (s.length()); - - ser << "AT\r"; - ser >> s; - if (s.compare("OK\r\n")) { - // not this serial port - exit(-1); - } - - ser << "AT+COEFF?\r"; - ser >> s; - if (s.compare("+COEFF=1\r\nOK\r\n")) { - // not this serial port - exit(-1); - } - - s = s.substr(14, s.length() - 14); - if (s.length() == 0) { - ser >> s; - } - // cout << s << endl; - cJSON *cparms = cJSON_ParseWithLength((const char *)s.c_str(), s.length()); - uint32_t tmp; - uvf_parms[0] = - ((float)((cJSON_GetObjectItem(cparms, "fx")->valueint) / 262144.0f)); - uvf_parms[1] = - ((float)((cJSON_GetObjectItem(cparms, "fy")->valueint) / 262144.0f)); - uvf_parms[2] = - ((float)((cJSON_GetObjectItem(cparms, "u0")->valueint) / 262144.0f)); - uvf_parms[3] = - ((float)((cJSON_GetObjectItem(cparms, "v0")->valueint) / 262144.0f)); - std::cout << "fx: " << uvf_parms[0] << std::endl; - std::cout << "fy: " << uvf_parms[1] << std::endl; - std::cout << "u0: " << uvf_parms[2] << std::endl; - std::cout << "v0: " << uvf_parms[3] << std::endl; - - /* do not delete it. It is waiting */ - ser >> s; - - ser << "AT+DISP=3\r"; - ser >> s; - if (s.compare("OK\r\n")) { - // not this serial port - exit(-1); - } - - publisher_depth = n->advertise("depth", 10); - publisher_pointcloud = n->advertise("cloud", 10); -} - -static void timer_callback() { - std::string s; - std::stringstream sstream; - frame_t *f; -_more: - ser >> s; - if (s.empty()) { - return; - } - f = handle_process(s); - if (!f) { - goto _more; - } - // cout << f << endl; - uint8_t rows, cols, *depth; - rows = f->frame_head.resolution_rows; - cols = f->frame_head.resolution_cols; - depth = f->payload; - cv::Mat md(rows, cols, CV_8UC1, depth); - - sstream << md.size(); - - std_msgs::Header header; - header.stamp = ros::Time::now(); - header.frame_id = "tof"; - - sensor_msgs::Image msg_depth = - *cv_bridge::CvImage(header, "mono8", md).toImageMsg().get(); - ROS_INFO("Publishing: depth:%s", sstream.str().c_str()); - publisher_depth.publish(msg_depth); - - sensor_msgs::PointCloud2 pcmsg; - pcmsg.header = header; - pcmsg.height = rows; - pcmsg.width = cols; - pcmsg.is_bigendian = false; - pcmsg.point_step = 16; - pcmsg.row_step = pcmsg.point_step * rows; - pcmsg.is_dense = false; - pcmsg.fields.resize(pcmsg.point_step / 4); - pcmsg.fields[0].name = "x"; - pcmsg.fields[0].offset = 0; - pcmsg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; - pcmsg.fields[0].count = 1; - pcmsg.fields[1].name = "y"; - pcmsg.fields[1].offset = 4; - pcmsg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; - pcmsg.fields[1].count = 1; - pcmsg.fields[2].name = "z"; - pcmsg.fields[2].offset = 8; - pcmsg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; - pcmsg.fields[2].count = 1; - pcmsg.fields[3].name = "rgb"; - pcmsg.fields[3].offset = 12; - pcmsg.fields[3].datatype = sensor_msgs::PointField::UINT32; - pcmsg.fields[3].count = 1; - - float fox = uvf_parms[0]; - float foy = uvf_parms[1]; - float u0 = uvf_parms[2]; - float v0 = uvf_parms[3]; - - pcmsg.data.resize((pcmsg.height) * (pcmsg.width) * (pcmsg.point_step), 0x00); - uint8_t *ptr = pcmsg.data.data(); - for (int j = 0; j < pcmsg.height; j++) - for (int i = 0; i < pcmsg.width; i++) { - float cx = (((float)i) - u0) / fox; - float cy = (((float)j) - v0) / foy; - float dst = ((float)depth[j * (pcmsg.width) + i]) / 1000; - float x = dst * cx; - float y = dst * cy; - float z = dst; - - *((float *)(ptr + 0)) = x; - *((float *)(ptr + 4)) = y; - *((float *)(ptr + 8)) = z; - const uint8_t *color = color_lut_jet[depth[j * (pcmsg.width) + i]]; - uint32_t color_r = color[0]; - uint32_t color_g = color[1]; - uint32_t color_b = color[2]; - *((uint32_t *)(ptr + 12)) = - (color_r << 16) | (color_g << 8) | (color_b << 0); - ptr += pcmsg.point_step; - } - publisher_pointcloud.publish(pcmsg); - - free(f); -} - -int main(int argc, char *argv[]) { - ros::init(argc, argv, "sipeed_tof"); - ros::NodeHandle n("~"); - ros::Rate loop_rate(30); - InitSipeedTOF_MSA010_Publisher(&n); - while (ros::ok()) { - timer_callback(); - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} - -const uint8_t color_lut_jet[][3] = { - {128, 0, 0}, {132, 0, 0}, {136, 0, 0}, {140, 0, 0}, - {144, 0, 0}, {148, 0, 0}, {152, 0, 0}, {156, 0, 0}, - {160, 0, 0}, {164, 0, 0}, {168, 0, 0}, {172, 0, 0}, - {176, 0, 0}, {180, 0, 0}, {184, 0, 0}, {188, 0, 0}, - {192, 0, 0}, {196, 0, 0}, {200, 0, 0}, {204, 0, 0}, - {208, 0, 0}, {212, 0, 0}, {216, 0, 0}, {220, 0, 0}, - {224, 0, 0}, {228, 0, 0}, {232, 0, 0}, {236, 0, 0}, - {240, 0, 0}, {244, 0, 0}, {248, 0, 0}, {252, 0, 0}, - {255, 0, 0}, {255, 4, 0}, {255, 8, 0}, {255, 12, 0}, - {255, 16, 0}, {255, 20, 0}, {255, 24, 0}, {255, 28, 0}, - {255, 32, 0}, {255, 36, 0}, {255, 40, 0}, {255, 44, 0}, - {255, 48, 0}, {255, 52, 0}, {255, 56, 0}, {255, 60, 0}, - {255, 64, 0}, {255, 68, 0}, {255, 72, 0}, {255, 76, 0}, - {255, 80, 0}, {255, 84, 0}, {255, 88, 0}, {255, 92, 0}, - {255, 96, 0}, {255, 100, 0}, {255, 104, 0}, {255, 108, 0}, - {255, 112, 0}, {255, 116, 0}, {255, 120, 0}, {255, 124, 0}, - {255, 128, 0}, {255, 132, 0}, {255, 136, 0}, {255, 140, 0}, - {255, 144, 0}, {255, 148, 0}, {255, 152, 0}, {255, 156, 0}, - {255, 160, 0}, {255, 164, 0}, {255, 168, 0}, {255, 172, 0}, - {255, 176, 0}, {255, 180, 0}, {255, 184, 0}, {255, 188, 0}, - {255, 192, 0}, {255, 196, 0}, {255, 200, 0}, {255, 204, 0}, - {255, 208, 0}, {255, 212, 0}, {255, 216, 0}, {255, 220, 0}, - {255, 224, 0}, {255, 228, 0}, {255, 232, 0}, {255, 236, 0}, - {255, 240, 0}, {255, 244, 0}, {255, 248, 0}, {255, 252, 0}, - {254, 255, 1}, {250, 255, 6}, {246, 255, 10}, {242, 255, 14}, - {238, 255, 18}, {234, 255, 22}, {230, 255, 26}, {226, 255, 30}, - {222, 255, 34}, {218, 255, 38}, {214, 255, 42}, {210, 255, 46}, - {206, 255, 50}, {202, 255, 54}, {198, 255, 58}, {194, 255, 62}, - {190, 255, 66}, {186, 255, 70}, {182, 255, 74}, {178, 255, 78}, - {174, 255, 82}, {170, 255, 86}, {166, 255, 90}, {162, 255, 94}, - {158, 255, 98}, {154, 255, 102}, {150, 255, 106}, {146, 255, 110}, - {142, 255, 114}, {138, 255, 118}, {134, 255, 122}, {130, 255, 126}, - {126, 255, 130}, {122, 255, 134}, {118, 255, 138}, {114, 255, 142}, - {110, 255, 146}, {106, 255, 150}, {102, 255, 154}, {98, 255, 158}, - {94, 255, 162}, {90, 255, 166}, {86, 255, 170}, {82, 255, 174}, - {78, 255, 178}, {74, 255, 182}, {70, 255, 186}, {66, 255, 190}, - {62, 255, 194}, {58, 255, 198}, {54, 255, 202}, {50, 255, 206}, - {46, 255, 210}, {42, 255, 214}, {38, 255, 218}, {34, 255, 222}, - {30, 255, 226}, {26, 255, 230}, {22, 255, 234}, {18, 255, 238}, - {14, 255, 242}, {10, 255, 246}, {6, 255, 250}, {2, 255, 254}, - {0, 252, 255}, {0, 248, 255}, {0, 244, 255}, {0, 240, 255}, - {0, 236, 255}, {0, 232, 255}, {0, 228, 255}, {0, 224, 255}, - {0, 220, 255}, {0, 216, 255}, {0, 212, 255}, {0, 208, 255}, - {0, 204, 255}, {0, 200, 255}, {0, 196, 255}, {0, 192, 255}, - {0, 188, 255}, {0, 184, 255}, {0, 180, 255}, {0, 176, 255}, - {0, 172, 255}, {0, 168, 255}, {0, 164, 255}, {0, 160, 255}, - {0, 156, 255}, {0, 152, 255}, {0, 148, 255}, {0, 144, 255}, - {0, 140, 255}, {0, 136, 255}, {0, 132, 255}, {0, 128, 255}, - {0, 124, 255}, {0, 120, 255}, {0, 116, 255}, {0, 112, 255}, - {0, 108, 255}, {0, 104, 255}, {0, 100, 255}, {0, 96, 255}, - {0, 92, 255}, {0, 88, 255}, {0, 84, 255}, {0, 80, 255}, - {0, 76, 255}, {0, 72, 255}, {0, 68, 255}, {0, 64, 255}, - {0, 60, 255}, {0, 56, 255}, {0, 52, 255}, {0, 48, 255}, - {0, 44, 255}, {0, 40, 255}, {0, 36, 255}, {0, 32, 255}, - {0, 28, 255}, {0, 24, 255}, {0, 20, 255}, {0, 16, 255}, - {0, 12, 255}, {0, 8, 255}, {0, 4, 255}, {0, 0, 255}, - {0, 0, 252}, {0, 0, 248}, {0, 0, 244}, {0, 0, 240}, - {0, 0, 236}, {0, 0, 232}, {0, 0, 228}, {0, 0, 224}, - {0, 0, 220}, {0, 0, 216}, {0, 0, 212}, {0, 0, 208}, - {0, 0, 204}, {0, 0, 200}, {0, 0, 196}, {0, 0, 192}, - {0, 0, 188}, {0, 0, 184}, {0, 0, 180}, {0, 0, 176}, - {0, 0, 172}, {0, 0, 168}, {0, 0, 164}, {0, 0, 160}, - {0, 0, 156}, {0, 0, 152}, {0, 0, 148}, {0, 0, 144}, - {0, 0, 140}, {0, 0, 136}, {0, 0, 132}, {0, 0, 128}}; diff --git a/sipeed_tof_ms_a010_ros/ros1/src/msa010.hpp b/sipeed_tof_ms_a010_ros/ros1/src/msa010.hpp new file mode 100644 index 0000000..97c93c9 --- /dev/null +++ b/sipeed_tof_ms_a010_ros/ros1/src/msa010.hpp @@ -0,0 +1,227 @@ +#include +#include + +using namespace boost; + +#include +#include +#include + +using namespace std::chrono; +using namespace std::literals::chrono_literals; + +#define DEFAULT_USRT_PORT "/dev/ttyUSB1" +#define DEFAULT_READ_TIMEDOUT 200 + +#define LOG_LINE \ + do { \ + std::cout << "[" \ + << duration_cast(high_resolution_clock::now() - \ + _start) \ + .count() * \ + 0.001 \ + << "][" << __LINE__ << "][" << std::this_thread::get_id() << ']' \ + << std::endl; \ + } while (0) +static std::chrono::time_point _start = + high_resolution_clock::now(); + +class msa010 { + private: + const char* _serial_path; + std::size_t _read_timedout; + asio::io_service _ioc; + asio::serial_port _sp; + asio::deadline_timer _timeout; + // asio::executor_work_guard _worker; + std::unique_ptr _worker; + std::thread _ioc_run_thread; + + public: + msa010() : msa010(DEFAULT_USRT_PORT) {} + msa010(const char* __serial_path) + : _serial_path(__serial_path), + _read_timedout(DEFAULT_READ_TIMEDOUT), + _ioc(), + _sp(_ioc), + _timeout(_ioc), + // _worker(asio::make_work_guard(_ioc)), + _worker(new asio::io_service::work(_ioc)), + _ioc_run_thread(std::bind([](asio::io_service& _ioc) { _ioc.run(); }, + std::ref(_ioc))) {} + + ~msa010() { + _worker.reset(); + _ioc_run_thread.join(); + }; + + inline bool is_connected() { return _sp.is_open(); } + + void keep_connect(const std::function& f) { + boost::system::error_code ec; + if (_sp.is_open()) return; + do { + ec = try_connect(); + // std::cout << "[INFO] Try connect to " << _serial_path << "..." + // << " Result: " << ec.message() << std::endl; + } while (f() && ec); + }; + + void keep_connect() { + keep_connect([] { + std::this_thread::sleep_for(1s); + return true; + }); + // boost::system::error_code ec; + // if (_sp.is_open()) return; + // do { + // ec = try_connect(); + // // std::cout << "[INFO] Try connect to " << _serial_path << "..." + // // << " Result: " << ec.message() << std::endl; + // std::this_thread::sleep_for(1s); + // } while (ec); + }; + + void set_read_timedout(std::size_t timeout) { + this->_read_timedout = timeout; + } + + template + std::size_t read_timedout(const MutableBufferSequence& buffers) { + return read_timedout(buffers, _read_timedout); + } + + template + std::size_t read_timedout(const MutableBufferSequence& buffers, + std::size_t ms) { + bool is_timeout = false; + bool had_data = false; + std::size_t transferred = 0; + + _timeout.expires_from_now(boost::posix_time::milliseconds(ms)); + _timeout.async_wait([this, &had_data, + &is_timeout](const boost::system::error_code& error) { + if (error) { + switch (error.value()) { + case boost::asio::error::operation_aborted: /* Operation canceled */ { + // Data was read and this timeout was canceled + // std::cout << "[WARN][Timeout] Cancelled, Has Data..." << + // std::endl; + had_data = true; + return; + } break; + default: + break; + } + } + + // timeout + if (this->_sp.is_open()) { + // std::cout << "[WARN][Timeout] Was Fired, No Data..." << std::endl; + this->_sp.cancel(); // will cause read_callback to fire with an error + } + is_timeout = true; + }); + + _sp.async_read_some(buffers, [this, &transferred]( + const boost::system::error_code& error, + std::size_t bytes_transferred) { + this->_timeout + .cancel(); // will cause wait_callback to fire with an error + if (error) { + // No data was read! + switch (error.value()) { + case boost::asio::error::eof: /* End of file */ { + /* disconnect */ + // std::cout << "[WARN][Serial] End of file..." << std::endl; + this->_sp.close(); + } break; + case boost::asio::error::bad_descriptor: /* Bad file descriptor */ { + // std::cout << "[WARN][Serial] Bad file descriptor..." << + // std::endl; + this->_sp.close(); + } break; + } + return; + } + + transferred = bytes_transferred; + }); + + while (!(is_timeout || had_data)) { + std::this_thread::sleep_for(20ms); + } + + return transferred; + } + + std::size_t read_some(std::string& s) { +#define MAX_SIZE ((25 * 25 + 22) * 4 * 4) + std::size_t len; + boost::system::error_code ec; + + s.reserve(MAX_SIZE); + len = this->_sp.read_some(asio::buffer(s, MAX_SIZE), ec); + if (ec) this->_sp.close(); + + return len; + } + + inline msa010& operator<<(const std::string& s) { + boost::system::error_code ec; + this->_sp.write_some(asio::buffer(s), ec); + if (ec) this->_sp.close(); + return *this; + } + + inline msa010& operator>>(std::string& s) { +#define MAX_SIZE ((25 * 25 + 22) * 4 * 4) + static char b[MAX_SIZE]; + std::size_t len; + + s.clear(); + len = read_timedout(asio::buffer(b)); + if (len) { + s.append(b, len); + } + + return *this; + } + + friend inline std::unique_ptr& operator<<( + std::unique_ptr& a010, const std::string& s) { + *a010 << s; + return a010; + } + + friend inline std::unique_ptr& operator>>( + std::unique_ptr& a010, std::string& s) { + *a010 >> s; + return a010; + } + + private: + boost::system::error_code try_connect() { + using sp = boost::asio::serial_port; + + boost::system::error_code ec; + _sp.open(_serial_path, ec); + if (ec) return ec; + try { + // 波特率 + _sp.set_option(sp::baud_rate(115200)); + // 字符大小 + _sp.set_option(sp::character_size(8)); + // 奇偶校验，可以为serial_port::parity::none / odd / even。 + _sp.set_option(sp::parity(sp::parity::none)); + // 停止位，可以为serial_port::stop_bits::one / onepointfive /two + _sp.set_option(sp::stop_bits(sp::stop_bits::one)); + // 流量控制，可以为serial_port::flow_control::type，enum类型，可以是none + _sp.set_option(sp::flow_control(sp::flow_control::none)); + } catch (boost::system::system_error const& ex) { + ec = ex.code(); + _sp.close(); + } + return ec; + } +}; \ No newline at end of file diff --git a/sipeed_tof_ms_a010_ros/ros1/src/msa010_auto_test.cc b/sipeed_tof_ms_a010_ros/ros1/src/msa010_auto_test.cc new file mode 100644 index 0000000..93e4bfe --- /dev/null +++ b/sipeed_tof_ms_a010_ros/ros1/src/msa010_auto_test.cc @@ -0,0 +1,102 @@ +#include +#include + +#include "cJSON.h" +#include "frame_struct.h" +#include "msa010.hpp" + +int main(int argc, char const *argv[]) { + auto a010 = std::make_unique(); + + std::string s; + while (1) { + a010->keep_connect(); + + a010 << "AT+DISP=1\r"; + do { + a010 >> s; + } while (s.size()); + + a010 << "AT\r"; + a010 >> s; + if (s.compare("OK\r\n")) { // not this serial port + continue; + } + + a010 << "AT+COEFF?\r"; + a010 >> s; + if (s.compare("+COEFF=1\r\nOK\r\n")) { // not this serial port + continue; + } + + a010 >> s; + if (!s.size()) { // not this serial port + continue; + } + + float uvf_parms[4]; + cJSON *cparms = cJSON_ParseWithLength((const char *)s.c_str(), s.length()); + uint32_t tmp; + uvf_parms[0] = + ((float)((cJSON_GetObjectItem(cparms, "fx")->valueint) / 262144.0f)); + uvf_parms[1] = + ((float)((cJSON_GetObjectItem(cparms, "fy")->valueint) / 262144.0f)); + uvf_parms[2] = + ((float)((cJSON_GetObjectItem(cparms, "u0")->valueint) / 262144.0f)); + uvf_parms[3] = + ((float)((cJSON_GetObjectItem(cparms, "v0")->valueint) / 262144.0f)); + std::cout << "fx: " << uvf_parms[0] << std::endl; + std::cout << "fy: " << uvf_parms[1] << std::endl; + std::cout << "u0: " << uvf_parms[2] << std::endl; + std::cout << "v0: " << uvf_parms[3] << std::endl; + + a010 << "AT+UNIT=2\r"; + a010 >> s; + if (s.compare("OK\r\n")) { // not this serial port + continue; + } + + std::this_thread::sleep_for(200ms); + + a010 << "AT+DISP=3\r"; + a010 >> s; + if (s.compare("OK\r\n")) { // not this serial port + continue; + } + + uint16_t times = 0; + + for (a010 >> s; s.size(); a010 >> s) { + // for (a010->read_some(s); s.size(); a010->read_some(s)) { + extern frame_t *handle_process(const std::string &s); + frame_t *f = handle_process(s); + if (!f) continue; + + cv::Mat src(f->frame_head.resolution_rows, f->frame_head.resolution_cols, + CV_8UC1, f->payload); + + std::stringstream fmt; + fmt << "./" << times << ".jpg"; + cv::imwrite(fmt.str(), src); + + std::cout << f << " " << fmt.str() << std::endl; + + free(f); + times += 1; + + if (times == 5) { + a010 << "AT+DISP=1\r"; + } + } + std::cout << "end" << std::endl; + + while (a010->is_connected()) { + a010 >> s; + if (s.size()) { + std::cerr << s; + std::cerr << "--------------ONCE--------------" << std::endl; + } + } + } + return 0; +} diff --git a/sipeed_tof_ms_a010_ros/ros1/src/msa010_cin_test.cc b/sipeed_tof_ms_a010_ros/ros1/src/msa010_cin_test.cc new file mode 100644 index 0000000..4237b9b --- /dev/null +++ b/sipeed_tof_ms_a010_ros/ros1/src/msa010_cin_test.cc @@ -0,0 +1,40 @@ + +#include +#include + +#include "msa010.hpp" + +int main(int argc, char const* argv[]) { + auto a010 = std::make_unique(); + + std::deque dq; + + std::thread th([&dq] { + while (true) { + std::string s; + std::cin >> s; + s.push_back('\r'); + dq.push_back(s); + } + }); + + while (1) { + a010->keep_connect(); + + while (a010->is_connected()) { + std::string s; + a010 >> s; + if (s.size()) { + std::cerr << s; + std::cerr << "--------------ONCE--------------" << std::endl; + } + + if (!dq.empty()) { + auto s = dq.front(); + a010 << s; + dq.pop_front(); + } + } + } + return 0; +} diff --git a/sipeed_tof_ms_a010_ros/ros1/src/serial.cc b/sipeed_tof_ms_a010_ros/ros1/src/serial.cc deleted file mode 100644 index 36aa9e6..0000000 --- a/sipeed_tof_ms_a010_ros/ros1/src/serial.cc +++ /dev/null @@ -1,182 +0,0 @@ -// private headers -#include "serial.hh" - -#include -#include -#include - -#ifndef WIN32 -#define MS_A010_DEFAULT_SERIAL_DEV_PATH "/dev/ttyUSB0" -#else -#error "NOT SUPPORTED WIN32 NOW" -#define MS_A010_DEFAULT_SERIAL_DEV_PATH "COMx" -#endif - -Serial::Serial() : Serial(MS_A010_DEFAULT_SERIAL_DEV_PATH) {} - -Serial::Serial(const std::string &dev_path) : dev_path(dev_path) { - if (this->dev_path.empty()) { - std::cerr << "Error: no dev path provided" << std::endl; - return; - } - if (NULL == (this->priv = this->configure_serial())) { - std::cerr << "Error: failed to configure serial device" << std::endl; - return; - } -} - -#ifndef WIN32 -Serial::~Serial() { delete (int *)this->priv; } -#else -#endif - -#ifndef WIN32 -// Linux headers -#include // Error integer and strerror() function -#include // Contains file controls like O_RDWR -#include // write(), read(), close() -static inline int serial_setup(int serial_port, unsigned int baudrate); - -void *Serial::configure_serial() { - int fd = -1; - - if ((fd = open(this->dev_path.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)) < 0) { - std::cerr << "path for serial can't open" << std::endl; - return NULL; - } - fcntl(fd, F_SETFL, 0); - - if (serial_setup(fd, 115200u) < 0) { - std::cerr << "setup serial failed" << std::endl; - close(fd); - return NULL; - } - - return (new int(fd)); -} - -const std::vector Serial::readBytes() const { - uint8_t read_buf[16 * 1024]; - ssize_t bytes_readed = -1; - if ((bytes_readed = read(*(int *)this->priv, read_buf, sizeof(read_buf))) < - 0) { - bytes_readed = 0; - } - return std::vector(read_buf, read_buf + bytes_readed); -} - -void Serial::writeBytes(const std::vector &vec) const { - write(*(int *)this->priv, vec.cbegin().base(), vec.size()); -} - -// C library headers -#include -// Linux headers -#include -#include // Contains POSIX terminal control definitions - -/* 115200, 8, N, 1 */ -static inline int serial_setup(int serial_port, unsigned int baudrate) { - struct termios tty; - // Read in existing settings, and handle any error - if (tcgetattr(serial_port, &tty) != 0) { - std::cerr << "Error" << errno << "from tcgetattr: " << strerror(errno) - << std::endl; - return -1; - } - tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity (most common) - tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in - // communication (most common) - tty.c_cflag &= ~CSIZE; // Clear all the size bits, then use one of the - // statements below - tty.c_cflag |= CS8; // 8 bits per byte (most common) - tty.c_cflag &= - ~CRTSCTS; // Disable RTS/CTS hardware flow control (most common) - tty.c_cflag |= - (CREAD | CLOCAL); // Turn on READ & ignore ctrl lines (CLOCAL = 1) - - tty.c_lflag &= ~ICANON; - tty.c_lflag &= ~ECHO; // Disable echo - tty.c_lflag &= ~ECHOE; // Disable erasure - tty.c_lflag &= ~ECHONL; // Disable new-line echo - tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP - - tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl - tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | - ICRNL); // Disable any special handling of received bytes - - tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes - // (e.g. newline chars) - tty.c_oflag &= - ~ONLCR; // Prevent conversion of newline to carriage return/line feed - - tty.c_cc[VMIN] = 0; - tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds), returning as - // soon as any data is received. - - cfsetspeed(&tty, baudrate); - // Save tty settings, also checking for error - if (tcsetattr(serial_port, TCSANOW, &tty) != 0) { - std::cerr << "Error" << errno << "from tcsetattr: " << strerror(errno) - << std::endl; - return -1; - } - return 0; -} -#else -#endif - -#if TEST_FUNC_ENABLE -static int test() { - Serial ser{}; - std::vector vec; - std::string s; - - bool cap = false; - - char curr_key; - while (1) { - std::cin >> curr_key; - std::cout << "[handler] curr_key: " << curr_key << std::endl; - switch (curr_key) { - case 'a': - ser << "AT\r"; - break; - case 's': - ser << "AT+DISP=3\r"; - cap = true; - break; - case 'p': - ser << "AT+DISP=1\r"; - ser >> s; - while (!s.empty()) { - ser >> s; - } - cap = false; - break; - case 'c': - ser << "AT+COEFF?\r"; - break; - case 'w': - std::cout << std::string(vec.begin(), vec.end()) << std::endl; - std::vector().swap(vec); - break; - } - - ser >> s; - if (cap) { - std::cout << "vec size: " << vec.size() << std::endl; - vec.insert(vec.end(), s.begin(), s.end()); - } else { - std::cout << s << std::endl; - } - } - - return 0; -} -#endif - -// int main(int argc, char const *argv[]) { -// test(); -// return 0; -// } diff --git a/sipeed_tof_ms_a010_ros/ros1/src/serial.hh b/sipeed_tof_ms_a010_ros/ros1/src/serial.hh deleted file mode 100644 index ef8e23d..0000000 --- a/sipeed_tof_ms_a010_ros/ros1/src/serial.hh +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include - -class Serial { - private: - const std::string &dev_path; - void *priv = NULL; - - void *configure_serial(); - const std::vector readBytes() const; - void writeBytes(const std::vector &vec) const; - - public: - Serial(); - Serial(const std::string &dev_path); - ~Serial(); - - inline Serial &operator>>(std::string &s) { - std::vector v = this->readBytes(); - std::string(v.cbegin(), v.cend()).swap(s); - return *this; - } - - inline Serial &operator<<(const std::string &s) { - this->writeBytes(std::vector(s.cbegin(), s.cend())); - return *this; - } -}; \ No newline at end of file diff --git a/sipeed_tof_ms_a010_ros/ros1/src/sipeed_tof_ms_a010_node.cc b/sipeed_tof_ms_a010_ros/ros1/src/sipeed_tof_ms_a010_node.cc new file mode 100644 index 0000000..ea6d3dc --- /dev/null +++ b/sipeed_tof_ms_a010_ros/ros1/src/sipeed_tof_ms_a010_node.cc @@ -0,0 +1,266 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include "cJSON.h" +#include "frame_struct.h" +#include "msa010.hpp" + +extern const uint8_t color_lut_jet[][3]; + +int main(int argc, char **argv) { + // 初始化一个叫“sipeed_tof_ms_a010_ros_topic_publisher”的ROS节点 + ros::init(argc, argv, "sipeed_tof_ms_a010_ros_topic_publisher"); + // 创建一个Nodehandle对象，用来与ROS进行通信 + ros::NodeHandle node_obj("~"); + std::string s; + node_obj.param("device", s, "/dev/ttyUSB0"); + std::cout << "use device: " << s << std::endl; + + auto a010 = std::make_unique(strdup(s.c_str())); + + // 创建一个topic发布节点，节点是number_publisher，发布的topic的名称是"/numbers",发布的数据类型是Int32。第二项是buffer + // size参数。 + std::string to_device(s.substr(5)); + + std::stringstream ss; + + ss.str(""); + ss << to_device << "/depth"; + ros::Publisher publisher_depth = + node_obj.advertise(strdup(ss.str().c_str()), 10); + + ss.str(""); + ss << to_device << "/cloud"; + ros::Publisher publisher_pointcloud = + node_obj.advertise(strdup(ss.str().c_str()), + 10); + + // 定义发送数据频率，如果频率高的话注意同时调高上一个buffer size + ros::Rate loop_rate(30); + + // 当按Ctrl+C时，ros::ok()会返回0，退出该while循环。 + + while (ros::ok()) { + ROS_INFO("Try connecting..."); + a010->keep_connect([]() { return ros::ok(); }); + if (!a010->is_connected()) { + break; + } + ROS_INFO("Connect success."); + + a010 << "AT+DISP=1\r"; + do { + a010 >> s; + } while (s.size()); + + a010 << "AT\r"; + a010 >> s; + if (s.compare("OK\r\n")) { // not this serial port + continue; + } + + a010 << "AT+COEFF?\r"; + a010 >> s; + if (s.compare("+COEFF=1\r\nOK\r\n")) { // not this serial port + continue; + } + + a010 >> s; + if (!s.size()) { // not this serial port + continue; + } + + float uvf_parms[4]; + cJSON *cparms = cJSON_ParseWithLength((const char *)s.c_str(), s.length()); + uint32_t tmp; + uvf_parms[0] = + ((float)((cJSON_GetObjectItem(cparms, "fx")->valueint) / 262144.0f)); + uvf_parms[1] = + ((float)((cJSON_GetObjectItem(cparms, "fy")->valueint) / 262144.0f)); + uvf_parms[2] = + ((float)((cJSON_GetObjectItem(cparms, "u0")->valueint) / 262144.0f)); + uvf_parms[3] = + ((float)((cJSON_GetObjectItem(cparms, "v0")->valueint) / 262144.0f)); + std::cout << "fx: " << uvf_parms[0] << std::endl; + std::cout << "fy: " << uvf_parms[1] << std::endl; + std::cout << "u0: " << uvf_parms[2] << std::endl; + std::cout << "v0: " << uvf_parms[3] << std::endl; + + a010 << "AT+UNIT=4\r"; + a010 >> s; + if (s.compare("OK\r\n")) { // not this serial port + continue; + } + + a010 << "AT+DISP=3\r"; + // a010 >> s; + // if (s.compare("OK\r\n")) { // not this serial port + // continue; + // } + + std::size_t count = 0; + frame_t *f; + while (ros::ok() && a010->is_connected()) { + a010 >> s; + if (!s.size()) continue; + + extern frame_t *handle_process(const std::string &s); + f = handle_process(s); + if (!f) continue; + count += 1; + + uint8_t rows, cols, *depth; + rows = f->frame_head.resolution_rows; + cols = f->frame_head.resolution_cols; + depth = f->payload; + cv::Mat md(rows, cols, CV_8UC1, depth); + + ROS_INFO("Publishing %8u's frame:%p", count, f); + std_msgs::Header header; + header.stamp = ros::Time::now(); + header.frame_id = "map"; + { + sensor_msgs::Image msg_depth = + *cv_bridge::CvImage(header, "mono8", md).toImageMsg().get(); + publisher_depth.publish(msg_depth); + } + { + sensor_msgs::PointCloud2 pcmsg; + pcmsg.header = header; + pcmsg.height = rows; + pcmsg.width = cols; + pcmsg.is_bigendian = false; + pcmsg.point_step = 16; + pcmsg.row_step = pcmsg.point_step * rows; + pcmsg.is_dense = false; + pcmsg.fields.resize(pcmsg.point_step / 4); + pcmsg.fields[0].name = "x"; + pcmsg.fields[0].offset = 0; + pcmsg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + pcmsg.fields[0].count = 1; + pcmsg.fields[1].name = "y"; + pcmsg.fields[1].offset = 4; + pcmsg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; + pcmsg.fields[1].count = 1; + pcmsg.fields[2].name = "z"; + pcmsg.fields[2].offset = 8; + pcmsg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; + pcmsg.fields[2].count = 1; + pcmsg.fields[3].name = "rgb"; + pcmsg.fields[3].offset = 12; + pcmsg.fields[3].datatype = sensor_msgs::PointField::UINT32; + pcmsg.fields[3].count = 1; + float fox = uvf_parms[0]; + float foy = uvf_parms[1]; + float u0 = uvf_parms[2]; + float v0 = uvf_parms[3]; + pcmsg.data.resize((pcmsg.height) * (pcmsg.width) * (pcmsg.point_step), + 0x00); + uint8_t *ptr = pcmsg.data.data(); + for (int j = 0; j < pcmsg.height; j++) + for (int i = 0; i < pcmsg.width; i++) { + float cx = (((float)i) - u0) / fox; + float cy = (((float)j) - v0) / foy; + float dst = ((float)depth[j * (pcmsg.width) + i]) / 1000; + // float x = dst * cx; + // float y = dst * cy; + // float z = dst; + float x = dst * cx; + float z = -dst * cy; + float y = dst; + *((float *)(ptr + 0)) = x; + *((float *)(ptr + 4)) = y; + *((float *)(ptr + 8)) = z; + const uint8_t *color = color_lut_jet[depth[j * (pcmsg.width) + i]]; + uint32_t color_r = color[0]; + uint32_t color_g = color[1]; + uint32_t color_b = color[2]; + *((uint32_t *)(ptr + 12)) = + (color_r << 16) | (color_g << 8) | (color_b << 0); + ptr += pcmsg.point_step; + } + publisher_pointcloud.publish(pcmsg); + } + + free(f); + // 读取和更新ROS topics，如果没有spinonce()或spin()，节点不会发布消息。 + ros::spinOnce(); + // 为了达到之前定义的发送频率，需要一个delay时间。 + loop_rate.sleep(); + } + } + + return 0; +} + +const uint8_t color_lut_jet[][3] = { + {128, 0, 0}, {132, 0, 0}, {136, 0, 0}, {140, 0, 0}, + {144, 0, 0}, {148, 0, 0}, {152, 0, 0}, {156, 0, 0}, + {160, 0, 0}, {164, 0, 0}, {168, 0, 0}, {172, 0, 0}, + {176, 0, 0}, {180, 0, 0}, {184, 0, 0}, {188, 0, 0}, + {192, 0, 0}, {196, 0, 0}, {200, 0, 0}, {204, 0, 0}, + {208, 0, 0}, {212, 0, 0}, {216, 0, 0}, {220, 0, 0}, + {224, 0, 0}, {228, 0, 0}, {232, 0, 0}, {236, 0, 0}, + {240, 0, 0}, {244, 0, 0}, {248, 0, 0}, {252, 0, 0}, + {255, 0, 0}, {255, 4, 0}, {255, 8, 0}, {255, 12, 0}, + {255, 16, 0}, {255, 20, 0}, {255, 24, 0}, {255, 28, 0}, + {255, 32, 0}, {255, 36, 0}, {255, 40, 0}, {255, 44, 0}, + {255, 48, 0}, {255, 52, 0}, {255, 56, 0}, {255, 60, 0}, + {255, 64, 0}, {255, 68, 0}, {255, 72, 0}, {255, 76, 0}, + {255, 80, 0}, {255, 84, 0}, {255, 88, 0}, {255, 92, 0}, + {255, 96, 0}, {255, 100, 0}, {255, 104, 0}, {255, 108, 0}, + {255, 112, 0}, {255, 116, 0}, {255, 120, 0}, {255, 124, 0}, + {255, 128, 0}, {255, 132, 0}, {255, 136, 0}, {255, 140, 0}, + {255, 144, 0}, {255, 148, 0}, {255, 152, 0}, {255, 156, 0}, + {255, 160, 0}, {255, 164, 0}, {255, 168, 0}, {255, 172, 0}, + {255, 176, 0}, {255, 180, 0}, {255, 184, 0}, {255, 188, 0}, + {255, 192, 0}, {255, 196, 0}, {255, 200, 0}, {255, 204, 0}, + {255, 208, 0}, {255, 212, 0}, {255, 216, 0}, {255, 220, 0}, + {255, 224, 0}, {255, 228, 0}, {255, 232, 0}, {255, 236, 0}, + {255, 240, 0}, {255, 244, 0}, {255, 248, 0}, {255, 252, 0}, + {254, 255, 1}, {250, 255, 6}, {246, 255, 10}, {242, 255, 14}, + {238, 255, 18}, {234, 255, 22}, {230, 255, 26}, {226, 255, 30}, + {222, 255, 34}, {218, 255, 38}, {214, 255, 42}, {210, 255, 46}, + {206, 255, 50}, {202, 255, 54}, {198, 255, 58}, {194, 255, 62}, + {190, 255, 66}, {186, 255, 70}, {182, 255, 74}, {178, 255, 78}, + {174, 255, 82}, {170, 255, 86}, {166, 255, 90}, {162, 255, 94}, + {158, 255, 98}, {154, 255, 102}, {150, 255, 106}, {146, 255, 110}, + {142, 255, 114}, {138, 255, 118}, {134, 255, 122}, {130, 255, 126}, + {126, 255, 130}, {122, 255, 134}, {118, 255, 138}, {114, 255, 142}, + {110, 255, 146}, {106, 255, 150}, {102, 255, 154}, {98, 255, 158}, + {94, 255, 162}, {90, 255, 166}, {86, 255, 170}, {82, 255, 174}, + {78, 255, 178}, {74, 255, 182}, {70, 255, 186}, {66, 255, 190}, + {62, 255, 194}, {58, 255, 198}, {54, 255, 202}, {50, 255, 206}, + {46, 255, 210}, {42, 255, 214}, {38, 255, 218}, {34, 255, 222}, + {30, 255, 226}, {26, 255, 230}, {22, 255, 234}, {18, 255, 238}, + {14, 255, 242}, {10, 255, 246}, {6, 255, 250}, {2, 255, 254}, + {0, 252, 255}, {0, 248, 255}, {0, 244, 255}, {0, 240, 255}, + {0, 236, 255}, {0, 232, 255}, {0, 228, 255}, {0, 224, 255}, + {0, 220, 255}, {0, 216, 255}, {0, 212, 255}, {0, 208, 255}, + {0, 204, 255}, {0, 200, 255}, {0, 196, 255}, {0, 192, 255}, + {0, 188, 255}, {0, 184, 255}, {0, 180, 255}, {0, 176, 255}, + {0, 172, 255}, {0, 168, 255}, {0, 164, 255}, {0, 160, 255}, + {0, 156, 255}, {0, 152, 255}, {0, 148, 255}, {0, 144, 255}, + {0, 140, 255}, {0, 136, 255}, {0, 132, 255}, {0, 128, 255}, + {0, 124, 255}, {0, 120, 255}, {0, 116, 255}, {0, 112, 255}, + {0, 108, 255}, {0, 104, 255}, {0, 100, 255}, {0, 96, 255}, + {0, 92, 255}, {0, 88, 255}, {0, 84, 255}, {0, 80, 255}, + {0, 76, 255}, {0, 72, 255}, {0, 68, 255}, {0, 64, 255}, + {0, 60, 255}, {0, 56, 255}, {0, 52, 255}, {0, 48, 255}, + {0, 44, 255}, {0, 40, 255}, {0, 36, 255}, {0, 32, 255}, + {0, 28, 255}, {0, 24, 255}, {0, 20, 255}, {0, 16, 255}, + {0, 12, 255}, {0, 8, 255}, {0, 4, 255}, {0, 0, 255}, + {0, 0, 252}, {0, 0, 248}, {0, 0, 244}, {0, 0, 240}, + {0, 0, 236}, {0, 0, 232}, {0, 0, 228}, {0, 0, 224}, + {0, 0, 220}, {0, 0, 216}, {0, 0, 212}, {0, 0, 208}, + {0, 0, 204}, {0, 0, 200}, {0, 0, 196}, {0, 0, 192}, + {0, 0, 188}, {0, 0, 184}, {0, 0, 180}, {0, 0, 176}, + {0, 0, 172}, {0, 0, 168}, {0, 0, 164}, {0, 0, 160}, + {0, 0, 156}, {0, 0, 152}, {0, 0, 148}, {0, 0, 144}, + {0, 0, 140}, {0, 0, 136}, {0, 0, 132}, {0, 0, 128}}; \ No newline at end of file diff --git a/sipeed_tof_ms_a010_ros/ros1/src/test.cc b/sipeed_tof_ms_a010_ros/ros1/src/test.cc deleted file mode 100644 index 95f6f5b..0000000 --- a/sipeed_tof_ms_a010_ros/ros1/src/test.cc +++ /dev/null @@ -1,83 +0,0 @@ -#include -#include -#include -#include - -#include "cJSON.h" -#include "frame_struct.h" -#include "serial.hh" - -extern frame_t *handle_process(std::string s); - -using namespace std; -static int test() { - Serial ser("/dev/ttyUSB1"); - string s; - - ser << "AT\r"; - ser >> s; - if (s.compare("OK\r\n")) { - // not this serial port - return -1; - } - - ser << "AT+COEFF?\r"; - ser >> s; - if (s.compare("+COEFF=1\r\nOK\r\n")) { - // not this serial port - return -1; - } - ser >> s; - // cout << s << endl; - cJSON *cparms = cJSON_ParseWithLength((const char *)s.c_str(), s.length()); - uint32_t tmp; - tmp = cJSON_GetObjectItem(cparms, "fx")->valueint; - cout << "fx: " << (tmp >> 18) << "." << (tmp & 0x3ffff) << endl; - tmp = cJSON_GetObjectItem(cparms, "fy")->valueint; - cout << "fy: " << (tmp >> 18) << "." << (tmp & 0x3ffff) << endl; - tmp = cJSON_GetObjectItem(cparms, "u0")->valueint; - cout << "u0: " << (tmp >> 18) << "." << (tmp & 0x3ffff) << endl; - tmp = cJSON_GetObjectItem(cparms, "v0")->valueint; - cout << "v0: " << (tmp >> 18) << "." << (tmp & 0x3ffff) << endl; - - /* do not delete it. It is waiting */ - ser >> s; - // usleep(2600); - - uint16_t times = 0; - - ser << "AT+DISP=3\r"; - ser >> s; - if (s.compare("OK\r\n")) { - // not this serial port - return -1; - } - for (ser >> s; !s.empty(); ser >> s) { - frame_t *f = handle_process(s); - if (!f) continue; - - cout << f << " "; - cv::Mat src(f->frame_head.resolution_rows, f->frame_head.resolution_cols, - CV_8UC1, f->payload); - - stringstream fmt; - fmt << "./" << times << ".jpg"; - cv::imwrite(fmt.str(), src); - - free(f); - times += 1; - cout << endl; - - if (times == 4) { - ser << "AT+DISP=1\r"; - } - } - - cout << "end" << endl; - return 0; -} - -int main(int argc, char const *argv[]) { - test(); - return 0; -}