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;
-}