Initial commit
This commit is contained in:
@@ -0,0 +1,94 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(lslidar_c16_driver)
|
||||
|
||||
add_compile_options(-std=c++14)
|
||||
set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
set(${PROJECT_NAME}_CATKIN_DEPS
|
||||
angles
|
||||
pcl_ros
|
||||
roscpp
|
||||
sensor_msgs
|
||||
tf
|
||||
dynamic_reconfigure
|
||||
lslidar_c16_msgs
|
||||
nodelet
|
||||
)
|
||||
|
||||
set(libpcap_LIBRARIES -lpcap)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
${${PROJECT_NAME}_CATKIN_DEPS}
|
||||
pcl_conversions
|
||||
rospy
|
||||
std_msgs
|
||||
genmsg
|
||||
cv_bridge
|
||||
message_generation
|
||||
)
|
||||
|
||||
find_package(Boost COMPONENTS signals)
|
||||
find_package(Boost REQUIRED COMPONENTS thread)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}
|
||||
${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
|
||||
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
|
||||
CATKIN_DEPENDS message_runtime std_msgs
|
||||
)
|
||||
|
||||
|
||||
add_library(lslidar_input_c16 src/input.cc)
|
||||
target_link_libraries(lslidar_input_c16
|
||||
${catkin_LIBRARIES}
|
||||
${libpcap_LIBRARIES})
|
||||
|
||||
add_library(lslidar_c16_driver src/lslidar_c16_driver.cpp)
|
||||
|
||||
target_link_libraries(lslidar_c16_driver
|
||||
lslidar_input_c16
|
||||
${catkin_LIBRARIES})
|
||||
|
||||
# build the nodelet version
|
||||
add_library(lslidar_c16_driver_nodelet src/lslidar_c16_driver_nodelet.cc src/lslidar_c16_driver.cpp)
|
||||
target_link_libraries(lslidar_c16_driver_nodelet
|
||||
lslidar_input_c16
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(lslidar_c16_driver_node src/lslidar_c16_driver_node.cpp)
|
||||
|
||||
if(catkin_EXPORTED_TARGETS)
|
||||
add_dependencies(lslidar_input_c16 ${catkin_EXPORTED_TARGETS})
|
||||
endif()
|
||||
|
||||
target_link_libraries(lslidar_c16_driver_node
|
||||
lslidar_c16_driver
|
||||
lslidar_input_c16
|
||||
${catkin_LIBRARIES}
|
||||
${libpcap_LIBRARIES}
|
||||
)
|
||||
|
||||
|
||||
install(TARGETS lslidar_input_c16 lslidar_c16_driver lslidar_c16_driver_nodelet lslidar_c16_driver_node
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
install(FILES
|
||||
nodelet_lslidar_c16.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
* This file is part of lslidar_c16 driver.
|
||||
*
|
||||
* The driver is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* The driver is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Input -- base class used to access the data independently of
|
||||
* its source
|
||||
*
|
||||
* InputSocket -- derived class reads live data from the device
|
||||
* via a UDP socket
|
||||
*
|
||||
* InputPCAP -- derived class provides a similar interface from a
|
||||
* PCAP dump
|
||||
*/
|
||||
|
||||
#ifndef __LSLIDAR_INPUT_H_
|
||||
#define __LSLIDAR_INPUT_H_
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <pcap.h>
|
||||
#include <netinet/in.h>
|
||||
#include <ros/ros.h>
|
||||
#include <lslidar_c16_msgs/LslidarC16Packet.h>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/file.h>
|
||||
#include <signal.h>
|
||||
#include <sensor_msgs/TimeReference.h>
|
||||
|
||||
namespace lslidar_c16_driver {
|
||||
static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC
|
||||
static uint16_t DIFOP_DATA_PORT_NUMBER = 2369; // lslidar default difop data port on PC
|
||||
/**
|
||||
* 从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
|
||||
* @brief The Input class,
|
||||
*
|
||||
* @param private_nh 一个NodeHandled,用于通过节点传递参数
|
||||
* @param port
|
||||
* @returns 0 if successful,
|
||||
* -1 if end of file
|
||||
* >0 if incomplete packet (is this possible?)
|
||||
*/
|
||||
class Input {
|
||||
public:
|
||||
Input(ros::NodeHandle private_nh, uint16_t port);
|
||||
|
||||
virtual ~Input() {
|
||||
}
|
||||
|
||||
virtual int getPacket(lslidar_c16_msgs::LslidarC16Packet *pkt, const double time_offset) = 0;
|
||||
|
||||
int getRpm(void);
|
||||
|
||||
int getReturnMode(void);
|
||||
|
||||
bool getUpdateFlag(void);
|
||||
|
||||
void clearUpdateFlag(void);
|
||||
|
||||
protected:
|
||||
ros::NodeHandle private_nh_;
|
||||
uint16_t port_;
|
||||
std::string devip_str_;
|
||||
int cur_rpm_;
|
||||
int return_mode_;
|
||||
bool npkt_update_flag_;
|
||||
bool add_multicast;
|
||||
std::string group_ip;
|
||||
};
|
||||
|
||||
/** @brief Live lslidar input from socket. */
|
||||
class InputSocket : public Input {
|
||||
public:
|
||||
InputSocket(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
|
||||
|
||||
virtual ~InputSocket();
|
||||
|
||||
virtual int getPacket(lslidar_c16_msgs::LslidarC16Packet *pkt, const double time_offset);
|
||||
|
||||
private:
|
||||
private:
|
||||
int sockfd_;
|
||||
in_addr devip_;
|
||||
|
||||
};
|
||||
|
||||
/** @brief lslidar input from PCAP dump file.
|
||||
*
|
||||
* Dump files can be grabbed by libpcap
|
||||
*/
|
||||
class InputPCAP : public Input {
|
||||
public:
|
||||
InputPCAP(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
|
||||
std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0);
|
||||
|
||||
virtual ~InputPCAP();
|
||||
|
||||
virtual int getPacket(lslidar_c16_msgs::LslidarC16Packet *pkt, const double time_offset);
|
||||
|
||||
private:
|
||||
ros::Rate packet_rate_;
|
||||
std::string filename_;
|
||||
pcap_t *pcap_;
|
||||
bpf_program pcap_packet_filter_;
|
||||
char errbuf_[PCAP_ERRBUF_SIZE];
|
||||
bool empty_;
|
||||
bool read_once_;
|
||||
bool read_fast_;
|
||||
double repeat_delay_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif // __LSLIDAR_INPUT_H
|
||||
+99
@@ -0,0 +1,99 @@
|
||||
/*
|
||||
* This file is part of lslidar_c16 driver.
|
||||
*
|
||||
* The driver is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* The driver is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef _LS_C16_DRIVER_H_
|
||||
#define _LS_C16_DRIVER_H_
|
||||
|
||||
#include <string>
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_ros/impl/transforms.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include "input.h"
|
||||
#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>
|
||||
#include <lslidar_c16_msgs/lslidar_c16_control.h>
|
||||
|
||||
namespace lslidar_c16_driver
|
||||
{
|
||||
class lslidarDriver
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief lslidarDriver
|
||||
* @param node raw packet output topic
|
||||
* @param private_nh 通过这个节点传参数
|
||||
*/
|
||||
lslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh);
|
||||
|
||||
~lslidarDriver();
|
||||
bool poll(void);
|
||||
void difopPoll(void);
|
||||
bool lslidarC16Control(lslidar_c16_msgs::lslidar_c16_control::Request &req,lslidar_c16_msgs::lslidar_c16_control::Response &res);
|
||||
bool bSwitch = true;
|
||||
|
||||
private:
|
||||
/// Callback for skip num for time synchronization
|
||||
void skipNumCallback(const std_msgs::Int32::ConstPtr& skip_num);
|
||||
bool SendPacketTolidar(bool power_switch);
|
||||
// configuration parameters
|
||||
struct
|
||||
{
|
||||
std::string frame_id; ///< tf frame ID
|
||||
std::string model; ///< device model name
|
||||
int npackets; ///< number of packets to collect
|
||||
double rpm; ///< device rotation rate (RPMs)
|
||||
double time_offset; ///< time in seconds added to each time stamp
|
||||
int cut_angle;
|
||||
int return_mode; //return wave number
|
||||
} config_;
|
||||
|
||||
boost::shared_ptr<Input> msop_input_;
|
||||
boost::shared_ptr<Input> difop_input_;
|
||||
ros::Publisher msop_output_;
|
||||
ros::Publisher difop_output_;
|
||||
ros::Publisher output_sync_;
|
||||
ros::ServiceServer lslidar_control;
|
||||
int switch_status;
|
||||
unsigned char difop_data[1206];
|
||||
// Converter convtor_
|
||||
boost::shared_ptr<boost::thread> difop_thread_;
|
||||
|
||||
// add for time synchronization
|
||||
bool time_synchronization_;
|
||||
unsigned char packetTimeStamp[10];
|
||||
uint64_t pointcloudTimeStamp;
|
||||
uint64_t GPSStableTS;
|
||||
uint64_t GPSCountingTS;
|
||||
uint64_t last_FPGA_ts;
|
||||
uint64_t GPS_ts;
|
||||
int cnt_gps_ts;
|
||||
ros::Time timeStamp;
|
||||
uint64_t usec_start;
|
||||
std::string device_ip_string;
|
||||
|
||||
bool scan_fill;
|
||||
lslidar_c16_msgs::LslidarC16ScanUnified scan_start;
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
} // namespace lslidar_driver
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,20 @@
|
||||
<launch>
|
||||
|
||||
<arg name="device_ip" value="192.168.1.200"/>
|
||||
<arg name="firing_port" value="2368"/>
|
||||
<arg name="topic_packet" default="/apollo/sensor/velodyne16/LslidarPacket"/>
|
||||
<arg name="topic_pointcloud" default="/apollo/sensor/velodyne16/PointCloud2"/>
|
||||
<!--arg name="manager_name" default="nodelet_manager"/-->
|
||||
<!-- start nodelet manager and load driver nodelet -->
|
||||
<node pkg="nodelet" type="nodelet" name="lslidar_c16_nodelet_manager" args="manager" output="screen" respawn="true"/>
|
||||
|
||||
<node pkg="nodelet" type="nodelet"
|
||||
name="lslidar_c16_driver_nodelet"
|
||||
args="load lslidar_c16_driver/DriverNodelet lslidar_c16_nodelet_manager" respawn="true">
|
||||
<param name="frame_id" value="laser_link"/>
|
||||
|
||||
|
||||
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,7 @@
|
||||
<launch>
|
||||
<node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver" >
|
||||
<param name="frame_id" value="lslidar"/>
|
||||
<param name="device_ip" value="192.168.1.200"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@@ -0,0 +1,9 @@
|
||||
<library path="lib/liblslidar_c16_driver_nodelet">
|
||||
<class name="lslidar_c16_driver/DriverNodelet"
|
||||
type="lslidar_c16_driver::DriverNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Publish one lslidar raw data packet each time.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -0,0 +1,58 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>lslidar_c16_driver</name>
|
||||
<version>1.0.0</version>
|
||||
|
||||
<maintainer email="tongsky723@sina.com">tongsky</maintainer>
|
||||
<author>Yutong</author>
|
||||
<license>BSD</license>
|
||||
<description>
|
||||
ROS device driver for Leishen C16 lidar.
|
||||
</description>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>angles</build_depend>
|
||||
<build_depend>nodelet</build_depend>
|
||||
<build_depend>pcl_conversions</build_depend>
|
||||
<build_depend>pcl_ros</build_depend>
|
||||
<build_depend>pluginlib</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>dynamic_reconfigure</build_depend>
|
||||
<build_depend>roslaunch</build_depend>
|
||||
<build_depend>rostest</build_depend>
|
||||
<build_depend>tf2_ros</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>rslidar_input</build_depend>
|
||||
<build_depend>pcl_conversions</build_depend>
|
||||
<build_depend>pcl_ros</build_depend>
|
||||
<build_depend>libpcl-all-dev</build_depend>
|
||||
<build_depend>lslidar_c16_msgs</build_depend>
|
||||
|
||||
|
||||
<run_depend>angles</run_depend>
|
||||
<run_depend>pcl_ros</run_depend>
|
||||
<run_depend>nodelet</run_depend>
|
||||
<run_depend>pluginlib</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>dynamic_reconfigure</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>pcl_conversions</run_depend>
|
||||
<run_depend>pcl_ros</run_depend>
|
||||
<run_depend>libpcl-all</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>lslidar_c16_msgs</run_depend>
|
||||
|
||||
<export>
|
||||
<nodelet plugin="${prefix}/nodelet_lslidar_c16.xml"/>
|
||||
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,386 @@
|
||||
/*
|
||||
* This file is part of lslidar_c16 driver.
|
||||
*
|
||||
* The driver is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* The driver is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include "lslidar_c16_driver/input.h"
|
||||
|
||||
extern volatile sig_atomic_t flag;
|
||||
namespace lslidar_c16_driver
|
||||
{
|
||||
static const size_t packet_size = sizeof(lslidar_c16_msgs::LslidarC16Packet().data);
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// Input base class implementation
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** @brief constructor
|
||||
*
|
||||
* @param private_nh ROS private handle for calling node.
|
||||
* @param port UDP port number.
|
||||
*/
|
||||
Input::Input(ros::NodeHandle private_nh, uint16_t port) : private_nh_(private_nh), port_(port)
|
||||
{
|
||||
npkt_update_flag_ = false;
|
||||
cur_rpm_ = 0;
|
||||
return_mode_ = 1;
|
||||
|
||||
private_nh.param("device_ip", devip_str_, std::string(""));
|
||||
private_nh.param<bool>("add_multicast", add_multicast, false);
|
||||
private_nh.param<std::string>("group_ip", group_ip, "224.1.1.2");
|
||||
if (!devip_str_.empty())
|
||||
ROS_INFO_STREAM("Only accepting packets from IP address: " << devip_str_);
|
||||
}
|
||||
|
||||
int Input::getRpm(void)
|
||||
{
|
||||
return cur_rpm_;
|
||||
}
|
||||
|
||||
int Input::getReturnMode(void)
|
||||
{
|
||||
return return_mode_;
|
||||
}
|
||||
|
||||
bool Input::getUpdateFlag(void)
|
||||
{
|
||||
return npkt_update_flag_;
|
||||
}
|
||||
|
||||
void Input::clearUpdateFlag(void)
|
||||
{
|
||||
npkt_update_flag_ = false;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// InputSocket class implementation
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** @brief constructor
|
||||
*
|
||||
* @param private_nh ROS private handle for calling node.
|
||||
* @param port UDP port number
|
||||
*/
|
||||
InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port) : Input(private_nh, port)
|
||||
{
|
||||
sockfd_ = -1;
|
||||
|
||||
if (!devip_str_.empty())
|
||||
{
|
||||
inet_aton(devip_str_.c_str(), &devip_);
|
||||
}
|
||||
|
||||
ROS_INFO_STREAM("Opening UDP socket: port " << port);
|
||||
sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
|
||||
if (sockfd_ == -1)
|
||||
{
|
||||
perror("socket"); // TODO: ROS_ERROR errno
|
||||
return;
|
||||
}
|
||||
|
||||
int opt = 1;
|
||||
if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void*)&opt, sizeof(opt)))
|
||||
{
|
||||
perror("setsockopt error!\n");
|
||||
return;
|
||||
}
|
||||
|
||||
sockaddr_in my_addr; // my address information
|
||||
memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
|
||||
my_addr.sin_family = AF_INET; // host byte order
|
||||
my_addr.sin_port = htons(port); // port in network byte order
|
||||
my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
|
||||
|
||||
if (bind(sockfd_, (sockaddr*)&my_addr, sizeof(sockaddr)) == -1)
|
||||
{
|
||||
perror("bind"); // TODO: ROS_ERROR errno
|
||||
return;
|
||||
}
|
||||
|
||||
if (add_multicast) {
|
||||
struct ip_mreq group;
|
||||
group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
|
||||
group.imr_interface.s_addr = htonl(INADDR_ANY);
|
||||
//group.imr_interface.s_addr = inet_addr("192.168.1.102");
|
||||
|
||||
if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
|
||||
perror("Adding multicast group error ");
|
||||
close(sockfd_);
|
||||
exit(1);
|
||||
} else
|
||||
printf("Adding multicast group...OK.\n");
|
||||
}
|
||||
if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
|
||||
{
|
||||
perror("non-block");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/** @brief destructor */
|
||||
InputSocket::~InputSocket(void)
|
||||
{
|
||||
(void)close(sockfd_);
|
||||
}
|
||||
|
||||
/** @brief Get one lslidar packet. */
|
||||
int InputSocket::getPacket(lslidar_c16_msgs::LslidarC16Packet* pkt, const double time_offset)
|
||||
{
|
||||
double time1 = ros::Time::now().toSec();
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sockfd_;
|
||||
fds[0].events = POLLIN;
|
||||
static const int POLL_TIMEOUT = 3000; // one second (in msec)
|
||||
|
||||
sockaddr_in sender_address;
|
||||
socklen_t sender_address_len = sizeof(sender_address);
|
||||
while (flag == 1)
|
||||
// while (true)
|
||||
{
|
||||
// Receive packets that should now be available from the
|
||||
// socket using a blocking read.
|
||||
// poll() until input available
|
||||
do
|
||||
{
|
||||
int retval = poll(fds, 1, POLL_TIMEOUT);
|
||||
if (retval < 0) // poll() error?
|
||||
{
|
||||
if (errno != EINTR)
|
||||
ROS_ERROR("poll() error: %s", strerror(errno));
|
||||
return 1;
|
||||
}
|
||||
if (retval == 0) // poll() timeout?
|
||||
{
|
||||
time_t curTime = time(NULL);
|
||||
struct tm *curTm = localtime(&curTime);
|
||||
char bufTime[30] = {0};
|
||||
sprintf(bufTime,"%d-%d-%d %d:%d:%d", curTm->tm_year+1900, curTm->tm_mon+1,
|
||||
curTm->tm_mday, curTm->tm_hour, curTm->tm_min, curTm->tm_sec);
|
||||
|
||||
ROS_WARN_THROTTLE(2, "%s lslidar poll() timeout", bufTime);
|
||||
/*
|
||||
char buffer_data[8] = "re-con";
|
||||
memset(&sender_address, 0, sender_address_len); // initialize to zeros
|
||||
sender_address.sin_family = AF_INET; // host byte order
|
||||
sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER); // port in network byte order, set any value
|
||||
sender_address.sin_addr.s_addr = devip_.s_addr; // automatically fill in my IP
|
||||
sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len);
|
||||
*/
|
||||
return 1;
|
||||
}
|
||||
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
|
||||
{
|
||||
ROS_ERROR("poll() reports lslidar error");
|
||||
return 1;
|
||||
}
|
||||
} while ((fds[0].revents & POLLIN) == 0);
|
||||
ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size, 0, (sockaddr*)&sender_address, &sender_address_len);
|
||||
|
||||
if (nbytes < 0)
|
||||
{
|
||||
if (errno != EWOULDBLOCK)
|
||||
{
|
||||
perror("recvfail");
|
||||
ROS_INFO("recvfail");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else if ((size_t)nbytes == packet_size)
|
||||
{
|
||||
if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
|
||||
continue;
|
||||
else
|
||||
break; // done
|
||||
}
|
||||
|
||||
ROS_DEBUG_STREAM("incomplete lslidar packet read: " << nbytes << " bytes");
|
||||
}
|
||||
if (flag == 0)
|
||||
{
|
||||
abort();
|
||||
}
|
||||
|
||||
if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A)
|
||||
{//difop
|
||||
|
||||
int rpm = (pkt->data[8]<<8)|pkt->data[9];
|
||||
if (rpm < 200){
|
||||
rpm = 600;
|
||||
}
|
||||
|
||||
//ROS_INFO("rpm=%d",rpm);
|
||||
|
||||
int mode = 1;
|
||||
if (cur_rpm_ != rpm || return_mode_ != mode)
|
||||
{
|
||||
cur_rpm_ = rpm;
|
||||
/* if((pkt->data[8] == 0x00) && (pkt->data[9] ==0x2f)){
|
||||
cur_rpm_ = 600;
|
||||
}*/
|
||||
return_mode_ = mode;
|
||||
|
||||
npkt_update_flag_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Average the times at which we begin and end reading. Use that to
|
||||
// estimate when the scan occurred. Add the time offset.
|
||||
double time2 = ros::Time::now().toSec();
|
||||
pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// InputPCAP class implementation
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/** @brief constructor
|
||||
*
|
||||
* @param private_nh ROS private handle for calling node.
|
||||
* @param port UDP port number
|
||||
* @param packet_rate expected device packet frequency (Hz)
|
||||
* @param filename PCAP dump file name
|
||||
*/
|
||||
InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_rate, std::string filename,
|
||||
bool read_once, bool read_fast, double repeat_delay)
|
||||
: Input(private_nh, port), packet_rate_(packet_rate), filename_(filename)
|
||||
{
|
||||
pcap_ = NULL;
|
||||
empty_ = true;
|
||||
|
||||
// get parameters using private node handle
|
||||
private_nh.param("read_once", read_once_, false);
|
||||
private_nh.param("read_fast", read_fast_, false);
|
||||
private_nh.param("repeat_delay", repeat_delay_, 0.0);
|
||||
|
||||
if (read_once_)
|
||||
ROS_INFO("Read input file only once.");
|
||||
if (read_fast_)
|
||||
ROS_INFO("Read input file as quickly as possible.");
|
||||
if (repeat_delay_ > 0.0)
|
||||
ROS_INFO("Delay %.3f seconds before repeating input file.", repeat_delay_);
|
||||
|
||||
// Open the PCAP dump file
|
||||
// ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
|
||||
ROS_INFO_STREAM("Opening PCAP file " << filename_);
|
||||
if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
|
||||
{
|
||||
ROS_FATAL("Error opening lslidar socket dump file.");
|
||||
return;
|
||||
}
|
||||
|
||||
std::stringstream filter;
|
||||
if (devip_str_ != "") // using specific IP?
|
||||
{
|
||||
filter << "src host " << devip_str_ << " && ";
|
||||
}
|
||||
filter << "udp dst port " << port;
|
||||
pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
|
||||
}
|
||||
|
||||
/** destructor */
|
||||
InputPCAP::~InputPCAP(void)
|
||||
{
|
||||
pcap_close(pcap_);
|
||||
}
|
||||
|
||||
/** @brief Get one lslidar packet. */
|
||||
int InputPCAP::getPacket(lslidar_c16_msgs::LslidarC16Packet* pkt, const double time_offset)
|
||||
{
|
||||
struct pcap_pkthdr* header;
|
||||
const u_char* pkt_data;
|
||||
|
||||
// while (flag == 1)
|
||||
while (true)
|
||||
{
|
||||
int res;
|
||||
if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
|
||||
{
|
||||
// Skip packets not for the correct port and from the
|
||||
// selected IP address.
|
||||
if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
|
||||
continue;
|
||||
|
||||
// Keep the reader from blowing through the file.
|
||||
if (read_fast_ == false)
|
||||
packet_rate_.sleep();
|
||||
|
||||
|
||||
memcpy(&pkt->data[0], pkt_data + 42, packet_size);
|
||||
|
||||
if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A)
|
||||
{//difop
|
||||
int rpm = (pkt->data[8]<<8)|pkt->data[9];
|
||||
int mode = 1;
|
||||
|
||||
if ((pkt->data[45] == 0x08 && pkt->data[46] == 0x02 && pkt->data[47] >= 0x09) || (pkt->data[45] > 0x08)
|
||||
|| (pkt->data[45] == 0x08 && pkt->data[46] > 0x02))
|
||||
{
|
||||
if (pkt->data[300] != 0x01 && pkt->data[300] != 0x02)
|
||||
{
|
||||
mode = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (cur_rpm_ != rpm || return_mode_ != mode)
|
||||
{
|
||||
cur_rpm_ = rpm;
|
||||
return_mode_ = mode;
|
||||
|
||||
npkt_update_flag_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
pkt->stamp = ros::Time::now(); // time_offset not considered here, as no
|
||||
// synchronization required
|
||||
empty_ = false;
|
||||
return 0; // success
|
||||
}
|
||||
|
||||
if (empty_) // no data in file?
|
||||
{
|
||||
ROS_WARN("Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (read_once_)
|
||||
{
|
||||
ROS_INFO("end of file reached -- done reading.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (repeat_delay_ > 0.0)
|
||||
{
|
||||
ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
|
||||
usleep(rint(repeat_delay_ * 1000000.0));
|
||||
}
|
||||
|
||||
ROS_DEBUG("replaying lslidar dump file");
|
||||
|
||||
// I can't figure out how to rewind the file, because it
|
||||
// starts with some kind of header. So, close the file
|
||||
// and reopen it with pcap.
|
||||
pcap_close(pcap_);
|
||||
pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
|
||||
empty_ = true; // maybe the file disappeared?
|
||||
} // loop back and try again
|
||||
|
||||
if (flag == 0)
|
||||
{
|
||||
abort();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,371 @@
|
||||
/*
|
||||
* Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
|
||||
* Copyright (C) 2009-2012 Austin Robot Technology, Jack O'Quin
|
||||
* Copyright (C) 2017 Robosense, Tony Zhang
|
||||
*
|
||||
* License: Modified BSD Software License Agreement
|
||||
*
|
||||
* $Id$
|
||||
*/
|
||||
|
||||
/** \file
|
||||
*
|
||||
* ROS driver implementation for the RILIDAR 3D LIDARs
|
||||
*/
|
||||
#include "lslidar_c16_driver/lslidar_c16_driver.h"
|
||||
#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>
|
||||
#include <lslidar_c16_driver/lslidar_c16_driver.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
|
||||
namespace lslidar_c16_driver {
|
||||
static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000;
|
||||
static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12;
|
||||
|
||||
lslidarDriver::lslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh) : switch_status(0) {
|
||||
scan_fill = false;
|
||||
// use private node handle to get parameters
|
||||
private_nh.param("frame_id", config_.frame_id, std::string("lslidar"));
|
||||
private_nh.param("device_ip", device_ip_string, std::string("192.168.1.200"));
|
||||
std::string tf_prefix = tf::getPrefixParam(private_nh);
|
||||
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
|
||||
config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
|
||||
|
||||
// get model name, validate string, determine packet rate
|
||||
private_nh.param("model", config_.model, std::string("LSC16"));
|
||||
double packet_rate; // packet frequency (Hz)
|
||||
|
||||
packet_rate = 840; //20000/24
|
||||
|
||||
private_nh.param("rpm", config_.rpm, 300.0);
|
||||
private_nh.param("return_mode", config_.return_mode, 1);
|
||||
double frequency = (config_.rpm / 60.0); // expected Hz rate
|
||||
printf("driver return mode = %d\n", config_.return_mode);
|
||||
// default number of packets for each scan is a single revolution
|
||||
// (fractions rounded up)
|
||||
int npackets = (int) ceil(packet_rate / frequency);
|
||||
private_nh.param("npackets", config_.npackets, npackets);
|
||||
ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
|
||||
|
||||
std::string dump_file;
|
||||
private_nh.param("pcap", dump_file, std::string(""));
|
||||
|
||||
int msop_udp_port;
|
||||
private_nh.param("msop_port", msop_udp_port, (int) MSOP_DATA_PORT_NUMBER);
|
||||
int difop_udp_port;
|
||||
private_nh.param("difop_port", difop_udp_port, (int) DIFOP_DATA_PORT_NUMBER);
|
||||
|
||||
scan_start = lslidar_c16_msgs::LslidarC16ScanUnified();
|
||||
scan_start.packets.resize(1);
|
||||
|
||||
// open rslidar input device or file
|
||||
if (dump_file != "") // have PCAP file?
|
||||
{
|
||||
// read data from packet capture file
|
||||
msop_input_.reset(new lslidar_c16_driver::InputPCAP(private_nh, msop_udp_port, packet_rate, dump_file));
|
||||
difop_input_.reset(new lslidar_c16_driver::InputPCAP(private_nh, difop_udp_port, packet_rate, dump_file));
|
||||
} else {
|
||||
// read data from live socket
|
||||
msop_input_.reset(new lslidar_c16_driver::InputSocket(private_nh, msop_udp_port));
|
||||
difop_input_.reset(new lslidar_c16_driver::InputSocket(private_nh, difop_udp_port));
|
||||
|
||||
}
|
||||
|
||||
// raw packet output topic
|
||||
std::string output_packets_topic;
|
||||
private_nh.param("output_packets_topic", output_packets_topic, std::string("lslidar_packet_c16"));
|
||||
msop_output_ = node.advertise<lslidar_c16_msgs::LslidarC16ScanUnified>(output_packets_topic, 10);
|
||||
|
||||
std::string output_difop_topic;
|
||||
private_nh.param("output_difop_topic", output_difop_topic, std::string("lslidar_packet_difop_c16"));
|
||||
difop_output_ = node.advertise<lslidar_c16_msgs::LslidarC16Packet>(output_difop_topic, 10);
|
||||
lslidar_control = node.advertiseService("lslidarcontrol", &lslidarDriver::lslidarC16Control, this);
|
||||
|
||||
difop_thread_ = boost::shared_ptr<boost::thread>(
|
||||
new boost::thread(boost::bind(&lslidarDriver::difopPoll, this)));
|
||||
private_nh.param("time_synchronization", time_synchronization_, false);
|
||||
|
||||
|
||||
if (time_synchronization_) {
|
||||
output_sync_ = node.advertise<sensor_msgs::TimeReference>("sync_header", 1);
|
||||
}
|
||||
memset(difop_data, 0, 1206);
|
||||
}
|
||||
|
||||
|
||||
lslidarDriver::~lslidarDriver() {
|
||||
if (difop_thread_ != NULL) {
|
||||
printf("error");
|
||||
difop_thread_->interrupt();
|
||||
difop_thread_->join();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/** poll the device
|
||||
*
|
||||
* @returns true unless end of file reached
|
||||
*/
|
||||
bool lslidarDriver::poll(void) { // Allocate a new shared pointer for zero-copy sharing with other nodelets.
|
||||
lslidar_c16_msgs::LslidarC16ScanUnifiedPtr scan(new lslidar_c16_msgs::LslidarC16ScanUnified);
|
||||
|
||||
// Since the rslidar delivers data at a very high rate, keep
|
||||
// reading and publishing scans as fast as possible.
|
||||
int mode = config_.return_mode;
|
||||
uint64_t GPSCurrentTS;
|
||||
if (difop_input_->getUpdateFlag()) {
|
||||
int packets_rate = ceil(POINTS_ONE_CHANNEL_PER_SECOND / BLOCKS_ONE_CHANNEL_PER_PKT);
|
||||
packets_rate = ceil(packets_rate / 2);
|
||||
config_.rpm = difop_input_->getRpm();
|
||||
|
||||
// if(config_.rpm >= 300 && config_.rpm < 600){
|
||||
// config_.rpm = 300;
|
||||
// }else if(config_.rpm >= 600 && config_.rpm < 1200){
|
||||
// config_.rpm = 600;
|
||||
// }else if(config_.rpm >= 1200){
|
||||
// config_.rpm = 1200;
|
||||
// }
|
||||
//config_.npackets = ceil(packets_rate * 60 / config_.rpm) * mode;
|
||||
config_.npackets = ceil(packets_rate * 60 / config_.rpm) * mode;
|
||||
|
||||
config_.npackets = config_.npackets * 11 / 10;
|
||||
|
||||
difop_input_->clearUpdateFlag();
|
||||
//ROS_INFO("packet rate is %d, rpm is %3.3f, npacket is %d", packets_rate, config_.rpm, config_.npackets);
|
||||
}
|
||||
|
||||
//ROS_INFO("rpm is %3.3f, npacket is %d", config_.rpm, config_.npackets);
|
||||
scan->packets.clear();
|
||||
scan->packets.resize(config_.npackets);
|
||||
int azi1, azi2;
|
||||
if (scan_fill) {
|
||||
scan->packets[0] = scan_start.packets[0];
|
||||
GPSCurrentTS = GPSCountingTS;
|
||||
} else {
|
||||
while (true) {
|
||||
while (true) {
|
||||
int rc = msop_input_->getPacket(&scan->packets[0], config_.time_offset);
|
||||
if (rc == 0)
|
||||
break;
|
||||
if (rc < 0)
|
||||
return false;
|
||||
}
|
||||
|
||||
azi1 = 256 * scan->packets[0].data[3] + scan->packets[0].data[2];
|
||||
azi2 = 256 * scan->packets[0].data[1103] + scan->packets[0].data[1102];
|
||||
if (azi1 > 35000 && azi2 < 1000) break;
|
||||
}
|
||||
}
|
||||
scan_fill = false;
|
||||
// use in standard behaviour only
|
||||
|
||||
for (int i = 1; i < config_.npackets; ++i) {
|
||||
|
||||
while (true) {
|
||||
// keep reading until full packet received
|
||||
//ROS_INFO_STREAM("time_offset: " << config_.time_offset);
|
||||
int rc = msop_input_->getPacket(&scan->packets[i], config_.time_offset);
|
||||
if (rc == 0)
|
||||
break; // got a full packet?
|
||||
if (rc < 0)
|
||||
return false; // end of file reached?
|
||||
|
||||
}
|
||||
azi1 = 256 * scan->packets[i].data[3] + scan->packets[i].data[2];
|
||||
azi2 = 256 * scan->packets[i].data[1103] + scan->packets[i].data[1102];
|
||||
//azi2 = (azi2 +20) % 36000;
|
||||
//if ( (azi1 > 35800 && azi2 < 100 )) {
|
||||
|
||||
if ((azi1 > 35000 && azi2 < 1000) || (azi1 < 500 && i > config_.npackets / 2)) {
|
||||
|
||||
scan_fill = true;
|
||||
scan_start.packets[0] = scan->packets[i];
|
||||
// ROS_INFO_STREAM("azi1: " << azi1 <<" "<< "azi2: " << azi2 << " i:" << i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (time_synchronization_) {
|
||||
sensor_msgs::TimeReference sync_header;
|
||||
|
||||
// it is already the msop msg
|
||||
// use the first packets
|
||||
lslidar_c16_msgs::LslidarC16Packet pkt = scan->packets[0];
|
||||
uint64_t packet_timestamp;
|
||||
|
||||
static uint64_t last_gps_time; //上一个设备包的gps时间
|
||||
static uint64_t last_packet_seconds; //上一个数据包的时间戳
|
||||
packet_timestamp = (pkt.data[1200] +
|
||||
pkt.data[1201] * pow(2, 8) +
|
||||
pkt.data[1202] * pow(2, 16) +
|
||||
pkt.data[1203] * pow(2, 24)) * 1e3; //ns
|
||||
|
||||
//timeStamp = ros::Time(GPSCurrentTS, packet_timestamp);// s,ns
|
||||
if (last_packet_seconds > 800000000 && packet_timestamp < 200000000) {
|
||||
timeStamp = ros::Time(GPSCurrentTS, packet_timestamp);
|
||||
} else {
|
||||
timeStamp = ros::Time(last_gps_time, packet_timestamp);
|
||||
}
|
||||
|
||||
last_gps_time = GPSCurrentTS;
|
||||
last_packet_seconds = packet_timestamp;
|
||||
|
||||
//ROS_INFO("Lidar_time: %f, GPS_time:%lu, fpga_time: ns:%lu",timeStamp.toSec(), GPSCurrentTS, packet_timestamp);
|
||||
sync_header.header.stamp = timeStamp;
|
||||
|
||||
output_sync_.publish(sync_header);
|
||||
}
|
||||
|
||||
// publish message using time of last packet read
|
||||
// ROS_INFO("Publishing a full scan.");
|
||||
if (time_synchronization_) {
|
||||
scan->header.stamp = timeStamp;
|
||||
|
||||
} else {
|
||||
//scan->header.stamp = scan->packets.back().stamp;
|
||||
scan->header.stamp = ros::Time::now();
|
||||
}
|
||||
scan->header.frame_id = config_.frame_id;
|
||||
msop_output_.publish(scan);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void lslidarDriver::difopPoll(void) {
|
||||
// reading and publishing scans as fast as possible.
|
||||
lslidar_c16_msgs::LslidarC16PacketPtr difop_packet_ptr(new lslidar_c16_msgs::LslidarC16Packet);
|
||||
while (ros::ok()) {
|
||||
// keep reading
|
||||
lslidar_c16_msgs::LslidarC16Packet difop_packet_msg;
|
||||
int rc = difop_input_->getPacket(&difop_packet_msg, config_.time_offset);
|
||||
if (rc == 0) {
|
||||
for (int i = 0; i < 1206; i++) {
|
||||
difop_data[i] = difop_packet_msg.data[i];
|
||||
}
|
||||
//std::cout << "Publishing a difop data." << std::endl;
|
||||
ROS_DEBUG("Publishing a difop data.");
|
||||
*difop_packet_ptr = difop_packet_msg;
|
||||
difop_output_.publish(difop_packet_ptr);
|
||||
int version_data = difop_packet_msg.data[1202];
|
||||
if (2 == version_data) {
|
||||
this->packetTimeStamp[4] = difop_packet_msg.data[41];
|
||||
this->packetTimeStamp[5] = difop_packet_msg.data[40];
|
||||
this->packetTimeStamp[6] = difop_packet_msg.data[39];
|
||||
this->packetTimeStamp[7] = difop_packet_msg.data[38];
|
||||
this->packetTimeStamp[8] = difop_packet_msg.data[37];
|
||||
this->packetTimeStamp[9] = difop_packet_msg.data[36];
|
||||
} else {
|
||||
this->packetTimeStamp[4] = difop_packet_msg.data[57];
|
||||
this->packetTimeStamp[5] = difop_packet_msg.data[56];
|
||||
this->packetTimeStamp[6] = difop_packet_msg.data[55];
|
||||
this->packetTimeStamp[7] = difop_packet_msg.data[54];
|
||||
this->packetTimeStamp[8] = difop_packet_msg.data[53];
|
||||
this->packetTimeStamp[9] = difop_packet_msg.data[52];
|
||||
}
|
||||
//ROS_INFO_STREAM("time: " << difop_packet_msg.data[57]);
|
||||
//ROS_INFO_STREAM("time: " << difop_packet_msg.data[56]);
|
||||
|
||||
struct tm cur_time;
|
||||
memset(&cur_time, 0, sizeof(cur_time));
|
||||
cur_time.tm_sec = this->packetTimeStamp[4] + 1;
|
||||
cur_time.tm_min = this->packetTimeStamp[5];
|
||||
cur_time.tm_hour = this->packetTimeStamp[6] + 8;
|
||||
cur_time.tm_mday = this->packetTimeStamp[7];
|
||||
cur_time.tm_mon = this->packetTimeStamp[8] - 1;
|
||||
cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
|
||||
this->pointcloudTimeStamp = mktime(&cur_time);
|
||||
|
||||
if (GPSCountingTS != this->pointcloudTimeStamp) {
|
||||
cnt_gps_ts = 0;
|
||||
GPSCountingTS = this->pointcloudTimeStamp;
|
||||
// ROS_ERROR("GPSCountingTS=%lu",GPSCountingTS);
|
||||
//to beijing time printing
|
||||
//ROS_INFO("GPS: y:%d m:%d d:%d h:%d m:%d s:%d",cur_time.tm_year+1900,cur_time.tm_mon+1,cur_time.tm_mday,cur_time.tm_hour+8,cur_time.tm_min,cur_time.tm_sec);
|
||||
} else if (cnt_gps_ts == 3) {
|
||||
GPSStableTS = GPSCountingTS;
|
||||
} else {
|
||||
cnt_gps_ts++;
|
||||
}
|
||||
}
|
||||
if (rc < 0)
|
||||
return; // end of file reached?
|
||||
ros::spinOnce();
|
||||
}
|
||||
}
|
||||
|
||||
bool lslidarDriver::lslidarC16Control(lslidar_c16_msgs::lslidar_c16_control::Request &req,
|
||||
lslidar_c16_msgs::lslidar_c16_control::Response &res) {
|
||||
ROS_WARN("--------------------------");
|
||||
// sleep(1);
|
||||
lslidar_c16_msgs::LslidarC16Packet packet0;
|
||||
packet0.data[0] = 0x00;
|
||||
packet0.data[1] = 0x00;
|
||||
int rc_msop = -1;
|
||||
|
||||
|
||||
if (req.LaserControl == 1) {
|
||||
|
||||
if ((rc_msop = msop_input_->getPacket(&packet0, config_.time_offset)) == 0) {
|
||||
res.status = "already power on status";
|
||||
return true;
|
||||
}
|
||||
ROS_WARN("receive cmd: %d,power on", req.LaserControl);
|
||||
SendPacketTolidar(true);
|
||||
double time1 = ros::Time::now().toSec();
|
||||
|
||||
do {
|
||||
rc_msop = msop_input_->getPacket(&packet0, config_.time_offset);
|
||||
double time2 = ros::Time::now().toSec();
|
||||
if(time2 - time1 > 20 ){
|
||||
res.status = "lidar connect error";
|
||||
return true;
|
||||
}
|
||||
} while ((rc_msop != 0) && (packet0.data[0] != 0xff) && (packet0.data[1] != 0xee));
|
||||
|
||||
res.status = "pow on";
|
||||
} else if (req.LaserControl == 0) {
|
||||
ROS_WARN("receive cmd: %d,power off", req.LaserControl);
|
||||
SendPacketTolidar(false);
|
||||
res.status = "power off";
|
||||
} else {
|
||||
res.status = "cmd error";
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
|
||||
}
|
||||
|
||||
bool lslidarDriver::SendPacketTolidar(bool power_switch) {
|
||||
int socketid;
|
||||
unsigned char config_data[1206];
|
||||
//int data_port = difop_data[24] * 256 + difop_data[25];
|
||||
mempcpy(config_data, difop_data, 1206);
|
||||
config_data[0] = 0xAA;
|
||||
config_data[1] = 0x00;
|
||||
config_data[2] = 0xFF;
|
||||
config_data[3] = 0x11;
|
||||
config_data[4] = 0x22;
|
||||
config_data[5] = 0x22;
|
||||
config_data[6] = 0xAA;
|
||||
config_data[7] = 0xAA;
|
||||
if (power_switch) {
|
||||
config_data[45] = 0x00;
|
||||
} else {
|
||||
config_data[45] = 0x01;
|
||||
}
|
||||
if (config_data[8] == 0x00) {
|
||||
config_data[8] = 0x02;
|
||||
config_data[9] = 0x58;
|
||||
}
|
||||
sockaddr_in addrSrv;
|
||||
socketid = socket(2, 2, 0);
|
||||
addrSrv.sin_addr.s_addr = inet_addr(device_ip_string.c_str());
|
||||
addrSrv.sin_family = AF_INET;
|
||||
addrSrv.sin_port = htons(2368);
|
||||
sendto(socketid, (const char *) config_data, 1206, 0, (struct sockaddr *) &addrSrv, sizeof(addrSrv));
|
||||
return 0;
|
||||
}
|
||||
|
||||
// add for time synchronization
|
||||
} // namespace lslidar_c16_driver
|
||||
@@ -0,0 +1,52 @@
|
||||
/*
|
||||
* This file is part of lslidar_c16 driver.
|
||||
*
|
||||
* The driver is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* The driver is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "lslidar_c16_driver/lslidar_c16_driver.h"
|
||||
#include "std_msgs/String.h"
|
||||
|
||||
using namespace lslidar_c16_driver;
|
||||
volatile sig_atomic_t flag = 1;
|
||||
|
||||
static void my_handler(int sig)
|
||||
{
|
||||
flag = 0;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "lslidar_c16_driver");
|
||||
ros::NodeHandle node;
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
signal(SIGINT, my_handler);
|
||||
|
||||
// start the driver
|
||||
lslidar_c16_driver::lslidarDriver dvr(node, private_nh);
|
||||
|
||||
// loop until shut down or end of file
|
||||
dvr.bSwitch = true;
|
||||
while (ros::ok())
|
||||
{
|
||||
if(dvr.bSwitch){
|
||||
dvr.poll();
|
||||
}
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* This file is part of lslidar_c16 driver.
|
||||
*
|
||||
* The driver is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* The driver is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
|
||||
#include "lslidar_c16_driver/lslidar_c16_driver.h"
|
||||
|
||||
volatile sig_atomic_t flag = 1;
|
||||
|
||||
namespace lslidar_c16_driver
|
||||
{
|
||||
class DriverNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
DriverNodelet() : running_(false)
|
||||
{
|
||||
}
|
||||
|
||||
~DriverNodelet()
|
||||
{
|
||||
if (running_)
|
||||
{
|
||||
NODELET_INFO("shutting down driver thread");
|
||||
running_ = false;
|
||||
deviceThread_->join();
|
||||
NODELET_INFO("driver thread stopped");
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void onInit(void);
|
||||
virtual void devicePoll(void);
|
||||
|
||||
volatile bool running_; ///< device thread is running
|
||||
boost::shared_ptr<boost::thread> deviceThread_;
|
||||
|
||||
boost::shared_ptr<lslidarDriver> dvr_; ///< driver implementation class
|
||||
};
|
||||
|
||||
void DriverNodelet::onInit()
|
||||
{
|
||||
// start the driver
|
||||
dvr_.reset(new lslidarDriver(getNodeHandle(), getPrivateNodeHandle()));
|
||||
|
||||
// spawn device poll thread
|
||||
running_ = true;
|
||||
deviceThread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
|
||||
// NODELET_INFO("DriverNodelet onInit");
|
||||
}
|
||||
|
||||
/** @brief Device poll thread main loop. */
|
||||
void DriverNodelet::devicePoll()
|
||||
{
|
||||
while (ros::ok() && dvr_->poll())
|
||||
{
|
||||
ros::spinOnce();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// parameters are: class type, base class type
|
||||
PLUGINLIB_EXPORT_CLASS(lslidar_c16_driver::DriverNodelet, nodelet::Nodelet)
|
||||
Reference in New Issue
Block a user