diff --git a/launch/rplidar_s1.launch b/launch/rplidar_s1.launch
index 11753abd..26668545 100644
--- a/launch/rplidar_s1.launch
+++ b/launch/rplidar_s1.launch
@@ -1,10 +1,9 @@
-
+
-
diff --git a/launch/rplidar_s1_tcp.launch b/launch/rplidar_s1_tcp.launch
new file mode 100644
index 00000000..a1e6b4bd
--- /dev/null
+++ b/launch/rplidar_s1_tcp.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/view_rplidar_s1_tcp.launch b/launch/view_rplidar_s1_tcp.launch
new file mode 100644
index 00000000..88b011df
--- /dev/null
+++ b/launch/view_rplidar_s1_tcp.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
diff --git a/src/node.cpp b/src/node.cpp
index d3442ea0..4b12ea6b 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -181,8 +181,11 @@ static float getAngle(const rplidar_response_measurement_node_hq_t& node)
int main(int argc, char * argv[]) {
ros::init(argc, argv, "rplidar_node");
-
+
+ std::string channel_type;
+ std::string tcp_ip;
std::string serial_port;
+ int tcp_port = 20108;
int serial_baudrate = 115200;
std::string frame_id;
bool inverted = false;
@@ -193,6 +196,9 @@ int main(int argc, char * argv[]) {
ros::NodeHandle nh;
ros::Publisher scan_pub = nh.advertise("scan", 1000);
ros::NodeHandle nh_private("~");
+ nh_private.param("channel_type", channel_type, "serial");
+ nh_private.param("tcp_ip", tcp_ip, "192.168.0.7");
+ nh_private.param("tcp_port", tcp_port, 20108);
nh_private.param("serial_port", serial_port, "/dev/ttyUSB0");
nh_private.param("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3
nh_private.param("frame_id", frame_id, "laser_frame");
@@ -205,20 +211,38 @@ int main(int argc, char * argv[]) {
u_result op_result;
// create the driver instance
- drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
+ if(channel_type == "tcp"){
+ drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP);
+ }
+ else{
+ drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
+ }
+
if (!drv) {
ROS_ERROR("Create Driver fail, exit");
return -2;
}
- // make connection...
- if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
- ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
- RPlidarDriver::DisposeDriver(drv);
- return -1;
+ if(channel_type == "tcp"){
+ // make connection...
+ if (IS_FAIL(drv->connect(tcp_ip.c_str(), (_u32)tcp_port))) {
+ ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
+ RPlidarDriver::DisposeDriver(drv);
+ return -1;
+ }
+
}
+ else{
+ // make connection...
+ if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
+ ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
+ RPlidarDriver::DisposeDriver(drv);
+ return -1;
+ }
+ }
+
// get rplidar device info
if (!getRPLIDARDeviceInfo(drv)) {
return -1;