Skip to content

Commit

Permalink
update ros1 for a010
Browse files Browse the repository at this point in the history
  • Loading branch information
@taorye
taorye committed Aug 11, 2023
1 parent fed5791 commit b754d61
Show file tree
Hide file tree
Showing 12 changed files with 749 additions and 618 deletions.
29 changes: 15 additions & 14 deletions sipeed_tof_ms_a010_ros/ros1/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
)

Expand All @@ -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
Expand All @@ -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}
)

Expand All @@ -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}
)

Expand Down Expand Up @@ -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()
Expand Down
56 changes: 47 additions & 9 deletions sipeed_tof_ms_a010_ros/ros1/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,29 +2,67 @@
<package format="2">
<name>sipeed_tof_ms_a010</name>
<version>0.0.0</version>
<description>ros1 support for MS-a010</description>
<maintainer email="[email protected]">taorye</maintainer>
<license>TODO: License declaration</license>
<description>The sipeed_tof_ms_a010 package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
<maintainer email="[email protected]">taorye</maintainer>


<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>


<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/sipeed_tof_ms_a010</url> -->


<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="[email protected]">Jane Doe</author> -->


<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>


<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->

</export>
</package>
</package>
65 changes: 37 additions & 28 deletions sipeed_tof_ms_a010_ros/ros1/src/frame_handle.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,13 @@

#include <algorithm>
// #include <iostream>
// using namespace std;
#include <numeric>
#include <string>
#include <vector>

#include "frame_struct.h"

frame_t *handle_process(std::string s) {
frame_t *handle_process(const std::string &s) {
static std::vector<uint8_t> vecChar;
static const uint8_t sflag_l = FRAME_BEGIN_FLAG & 0xff;
static const uint8_t sflag_h = (FRAME_BEGIN_FLAG >> 8) & 0xff;
Expand All @@ -17,36 +18,35 @@ frame_t *handle_process(std::string s) {
frame_t *pf = NULL;
std::vector<uint8_t>::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;
goto __finished;
}

__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<uint8_t>(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<uint8_t>(it - 1, vecChar.end()).swap(vecChar);
if (it != vecChar.begin()) {
std::vector<uint8_t>(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;
}

Expand All @@ -55,37 +55,46 @@ 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<uint8_t>(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;
}
}

pf = (frame_t *)malloc(sizeof(frame_t) + frame_payload_len);
memcpy(pf, &vecChar[0], sizeof(frame_t) + frame_payload_len);

std::vector<uint8_t>(it + FRAME_HEAD_SIZE + frame_payload_len +
FRAME_CHECKSUM_SIZE + FRAME_END_SIZE - 1,
std::vector<uint8_t>(vecChar.begin() + FRAME_HEAD_SIZE + frame_payload_len +
FRAME_CHECKSUM_SIZE + FRAME_END_SIZE,
vecChar.end())
.swap(vecChar);
return pf;
Expand Down
29 changes: 15 additions & 14 deletions sipeed_tof_ms_a010_ros/ros1/src/frame_struct.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down
Loading

0 comments on commit b754d61

Please sign in to comment.