Skip to content

Commit

Permalink
Merge branch 'dev' into release
Browse files Browse the repository at this point in the history
  • Loading branch information
@ronzheng1006
ronzheng1006 committed Aug 10, 2022
2 parents ecfb48a + 20f9b1b commit dfa78c9
Show file tree
Hide file tree
Showing 9 changed files with 55 additions and 238 deletions.
4 changes: 2 additions & 2 deletions demo/demo_online.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ int main(int argc, char* argv[])
RSDriverParam param; ///< Create a parameter object
param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699
param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788
param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct
param.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct
param.print();

driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver
Expand All @@ -95,4 +95,4 @@ int main(int argc, char* argv[])
}

return 0;
}
}
4 changes: 2 additions & 2 deletions demo/demo_pcap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ int main(int argc, char* argv[])
param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file directory
param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699
param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788
param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct
param.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct
param.print();

driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver
Expand All @@ -97,4 +97,4 @@ int main(int argc, char* argv[])
}

return 0;
}
}
5 changes: 4 additions & 1 deletion src/rs_driver/common/error_code.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,8 @@ enum ErrCode
ERRCODE_DIFOPPORTBUZY = 0x50, ///< Input difop port is already used
ERRCODE_WRONGPKTHEADER = 0x51, ///< Packet header is wrong
ERRCODE_PKTNULL = 0x52, ///< Input packet is null
ERRCODE_PKTBUFOVERFLOW = 0x53 ///< Packet buffer is over flow
ERRCODE_PKTBUFOVERFLOW = 0x53, ///< Packet buffer is overflow
ERRCODE_CLOUDOVERFLOW = 0x54 ///< Point Cloud is overflow
};

struct Error
Expand Down Expand Up @@ -122,6 +123,8 @@ struct Error
return "ERRCODE_PKTNULL";
case ERRCODE_PKTBUFOVERFLOW:
return "ERRCODE_PKTBUFOVERFLOW";
case ERRCODE_CLOUDOVERFLOW:
return "ERRCODE_CLOUDOVERFLOW";
default:
return "ERRCODE_SUCCESS";
}
Expand Down
199 changes: 0 additions & 199 deletions src/rs_driver/driver/decoder/decoder_RS128_40.hpp
View file

This file was deleted.

25 changes: 6 additions & 19 deletions src/rs_driver/driver/decoder/decoder_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -744,12 +744,12 @@ template <typename T_Point>
inline void DecoderBase<T_Point>::transformPoint(float& x, float& y, float& z)
{
#ifdef ENABLE_TRANSFORM
Eigen::AngleAxisd current_rotation_x(param_.transform_param.roll, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd current_rotation_y(param_.transform_param.pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd current_rotation_z(param_.transform_param.yaw, Eigen::Vector3d::UnitZ());
Eigen::Translation3d current_translation(param_.transform_param.x, param_.transform_param.y,
param_.transform_param.z);
Eigen::Matrix4d trans = (current_translation * current_rotation_z * current_rotation_y * current_rotation_x).matrix();
static Eigen::AngleAxisd current_rotation_x(param_.transform_param.roll, Eigen::Vector3d::UnitX());
static Eigen::AngleAxisd current_rotation_y(param_.transform_param.pitch, Eigen::Vector3d::UnitY());
static Eigen::AngleAxisd current_rotation_z(param_.transform_param.yaw, Eigen::Vector3d::UnitZ());
static Eigen::Translation3d current_translation(param_.transform_param.x, param_.transform_param.y,
param_.transform_param.z);
static Eigen::Matrix4d trans = (current_translation * current_rotation_z * current_rotation_y * current_rotation_x).matrix();
Eigen::Vector4d target_ori(x, y, z, 1);
Eigen::Vector4d target_rotate = trans * target_ori;
x = target_rotate(0);
Expand Down Expand Up @@ -847,19 +847,6 @@ inline RSEchoMode DecoderBase<T_Point>::getEchoMode(const LidarType& type, const
{
switch (type)
{
case LidarType::RS128_40:
switch (return_mode)
{
case 0x00:
case 0x01:
case 0x02:
return RSEchoMode::ECHO_SINGLE;
case 0x03:
case 0x04:
case 0x05:
default:
return RSEchoMode::ECHO_DUAL;
}
case LidarType::RS128:
case LidarType::RS80:
case LidarType::RSHELIOS:
Expand Down
29 changes: 23 additions & 6 deletions src/rs_driver/driver/decoder/decoder_factory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rs_driver/driver/decoder/decoder_RS32.hpp>
#include <rs_driver/driver/decoder/decoder_RS80.hpp>
#include <rs_driver/driver/decoder/decoder_RS128.hpp>
#include <rs_driver/driver/decoder/decoder_RS128_40.hpp>
#include <rs_driver/driver/decoder/decoder_RSBP.hpp>
#include <rs_driver/driver/decoder/decoder_RSM1.hpp>
#include <rs_driver/driver/decoder/decoder_RSHELIOS.hpp>
Expand All @@ -60,9 +59,10 @@ class DecoderFactory
static const LidarConstantParameter getRSBPConstantParam();
static const LidarConstantParameter getRS80ConstantParam();
static const LidarConstantParameter getRS128ConstantParam();
static const LidarConstantParameter getRS128_40ConstantParam();
static const LidarConstantParameter getRSRUBY_PLUSConstantParam();
static const LidarConstantParameter getRSM1ConstantParam();
static const LidarConstantParameter getRSHELIOSConstantParam();
static const LidarConstantParameter getRSHELIOS_16PConstantParam();
static const LidarConstantParameter getRSROCKConstantParam();
};

Expand All @@ -84,9 +84,6 @@ inline std::shared_ptr<DecoderBase<T_Point>> DecoderFactory<T_Point>::createDeco
case LidarType::RS128:
ret_ptr = std::make_shared<DecoderRS128<T_Point>>(param.decoder_param, getRS128ConstantParam());
break;
case LidarType::RS128_40:
ret_ptr = std::make_shared<DecoderRS128_40<T_Point>>(param.decoder_param, getRS128_40ConstantParam());
break;
case LidarType::RS80:
ret_ptr = std::make_shared<DecoderRS80<T_Point>>(param.decoder_param, getRS80ConstantParam());
break;
Expand Down Expand Up @@ -208,7 +205,7 @@ inline const LidarConstantParameter DecoderFactory<T_Point>::getRS128ConstantPar
}

template <typename T_Point>
inline const LidarConstantParameter DecoderFactory<T_Point>::getRS128_40ConstantParam()
inline const LidarConstantParameter DecoderFactory<T_Point>::getRSRUBY_PLUSConstantParam()
{
LidarConstantParameter ret_param;
ret_param.MSOP_ID = 0x5A05AA55;
Expand Down Expand Up @@ -260,6 +257,26 @@ inline const LidarConstantParameter DecoderFactory<T_Point>::getRSHELIOSConstant
return ret_param;
}

template <typename T_Point>
inline const LidarConstantParameter DecoderFactory<T_Point>::getRSHELIOS_16PConstantParam()
{
LidarConstantParameter ret_param;
ret_param.MSOP_ID = 0x5A05AA55;
ret_param.DIFOP_ID = 0x555511115A00FFA5;
ret_param.BLOCK_ID = 0xEEFF;
ret_param.PKT_RATE = 750;
ret_param.BLOCKS_PER_PKT = 12;
ret_param.CHANNELS_PER_BLOCK = 32;
ret_param.LASER_NUM = 16;
ret_param.DSR_TOFFSET = 1.0;
ret_param.FIRING_FREQUENCY = 0.018;
ret_param.DIS_RESOLUTION = 0.0025;
ret_param.RX = 0.03498;
ret_param.RY = -0.015;
ret_param.RZ = 0.0;
return ret_param;
}

// TODO
template <typename T_Point>
inline const LidarConstantParameter DecoderFactory<T_Point>::getRSROCKConstantParam()
Expand Down
Loading

0 comments on commit dfa78c9

Please sign in to comment.