add pilottoros and rostopilot. for commulication between ros and our system.

src/ros/catkin/src/pilottoros/CMakeLists.txt

@@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(pilottoros)
+
+
+message(STATUS  "Enter testrosmodulecomm")
+
+
+SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+
+
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore)
+
+if(QT_EXIST)
+include_directories(
+  ${QT_PATH}/include
+  ${QT_PATH}/include/QtCore
+)
+link_directories(
+  ${QT_PATH}/lib
+)
+else()
+  message(FATAL_ERROR "Please set QT_PATH")
+endif()
+
+
+find_path(MODULECOMM_INCLUDE_DIR modulecomm.h  ${CMAKE_SOURCE_DIR}/../../../../include )
+find_library(MODULECOMM_LIBRARAY_DIR  modulecomm  ${CMAKE_SOURCE_DIR}/../../../../bin )
+
+if (MODULECOMM_INCLUDE_DIR  AND  MODULECOMM_LIBRARAY_DIR )
+  message(STATUS  "FIND  modulecomm")
+  include_directories(
+    ${CMAKE_SOURCE_DIR}/../../../../include
+  )
+  link_directories(
+    ${CMAKE_SOURCE_DIR}/../../../../bin
+)
+  else( )
+    message(FATAL_ERROR "Not Found modulecomm")
+endif ()
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  sensor_msgs
+  pcl_ros
+  pcl_conversions
+)
+find_package(Boost REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+##   INCLUDE_DIRS include
+#  LIBRARIES client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+    pcl_ros pcl_conversions
+  DEPENDS
+    Boost
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  ${Boost_INCLUDE_DIR}
+)
+
+
+## Declare a C++ executable
+add_executable(${PROJECT_NAME}_node src/main.cpp )
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} Qt5Core Qt5Xml modulecomm)
+

src/ros/catkin/src/pilottoros/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package>
+  <name>pilottoros</name>
+  <version>0.0.0</version>
+  <description>The pilottoros package for covert adciv pilot message to ros</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="hans@todo.todo">hans</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 mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <build_depend>pcl_ros</build_depend>
+  <run_depend>pcl_ros</run_depend>
+
+  <build_depend>pcl_conversions</build_depend>
+  <run_depend>pcl_conversions</run_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -0,0 +1,85 @@
+#include "ros/ros.h"
+
+#include "modulecomm.h"
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+static std::string _point_topic = "/points_raw";
+static std::string _point_msgname = "lidar_pc";
+
+ros::Publisher  point_cloud_pub;
+
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+	std::cout<<" get msg."<<std::endl;
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+	qint64 timestamp;
+	    memcpy(&timestamp,(char *)strdata+4+nNameSize+4,8);
+		double ftimestamp = timestamp;
+		ftimestamp = ftimestamp/1000.0;
+		point_cloud->header.stamp = ftimestamp;
+   // memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+
+	point_cloud->resize(nPCount);
+	memcpy(point_cloud->points.data(),p,nPCount*sizeof(pcl::PointXYZI));
+
+	/*
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        xp.x = p->x;
+        xp.y = p->y;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+    }
+	*/
+
+    point_cloud_pub.publish(point_cloud);
+}
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "pilottoros");
+	ros::NodeHandle n;
+	ros::Rate loop_rate(1);
+
+	    ros::NodeHandle private_nh("~");
+    private_nh.getParam("points_node",_point_topic);
+	private_nh.getParam("points_msgname",_point_msgname);
+    std::cout<<"_point_topic is "<<_point_topic<<std::endl;
+	std::cout<<"_point_msgname : "<<_point_msgname<<std::endl;
+    point_cloud_pub = n.advertise<sensor_msgs::PointCloud2>(
+                _point_topic, 10);
+
+	void * pliar = iv::modulecomm::RegisterRecv(_point_msgname.data(),ListenPointCloud);
+
+
+	while (ros::ok())
+	{
+ //       ROS_INFO("hello");
+        ros::spinOnce();
+	 	loop_rate.sleep();
+	}
+}

src/ros/catkin/src/rostopilot/CMakeLists.txt

@@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(rostopilot)
+
+
+message(STATUS  "Enter rostopilot")
+
+
+SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+
+
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore)
+
+if(QT_EXIST)
+include_directories(
+  ${QT_PATH}/include
+  ${QT_PATH}/include/QtCore
+)
+link_directories(
+  ${QT_PATH}/lib
+)
+else()
+  message(FATAL_ERROR "Please set QT_PATH")
+endif()
+
+
+find_path(MODULECOMM_INCLUDE_DIR modulecomm.h  ${CMAKE_SOURCE_DIR}/../../../../include )
+find_library(MODULECOMM_LIBRARAY_DIR  modulecomm  ${CMAKE_SOURCE_DIR}/../../../../bin )
+
+if (MODULECOMM_INCLUDE_DIR  AND  MODULECOMM_LIBRARAY_DIR )
+  message(STATUS  "FIND  modulecomm")
+  include_directories(
+    ${CMAKE_SOURCE_DIR}/../../../../include
+  )
+  link_directories(
+    ${CMAKE_SOURCE_DIR}/../../../../bin
+)
+  else( )
+    message(FATAL_ERROR "Not Found modulecomm")
+endif ()
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  sensor_msgs
+  pcl_ros
+  pcl_conversions
+)
+find_package(Boost REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+##   INCLUDE_DIRS include
+#  LIBRARIES client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+    pcl_ros pcl_conversions
+  DEPENDS
+    Boost
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  ${Boost_INCLUDE_DIR}
+)
+
+
+## Declare a C++ executable
+add_executable(${PROJECT_NAME}_node src/main.cpp )
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} Qt5Core Qt5Xml modulecomm)
+

src/ros/catkin/src/rostopilot/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package>
+  <name>rostopilot</name>
+  <version>0.0.0</version>
+  <description>The rostopilot package for covert ros message to adciv pilot message</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="hans@todo.todo">hans</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 mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <build_depend>pcl_ros</build_depend>
+  <run_depend>pcl_ros</run_depend>
+
+  <build_depend>pcl_conversions</build_depend>
+  <run_depend>pcl_conversions</run_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/rostopilot/src/main.cpp

@@ -0,0 +1,66 @@
+#include "ros/ros.h"
+
+#include "modulecomm.h"
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+static std::string _point_topic = "/points_raw";
+static std::string _point_msgname = "lidarpc_ros";
+
+  ros::Subscriber points_sub;
+  void * gpa_lidar;
+
+static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
+{
+    pcl::PointCloud<pcl::PointXYZI>  point_cloud;
+
+    ros::Time current_scan_time = input->header.stamp;
+    static ros::Time previous_scan_time = current_scan_time;
+
+    pcl::fromROSMsg(*input, point_cloud);
+
+        char * strOut = new char[4+sizeof(point_cloud.header.frame_id.size()) + 4+8+point_cloud.width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud.header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud.header.frame_id.c_str(),point_cloud.header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud.header.frame_id.size()));
+    *pu32 = point_cloud.header.seq;
+    memcpy(strOut+4+sizeof(point_cloud.header.frame_id.size()) + 4,&point_cloud.header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud.header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud.points.data(),point_cloud.width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(gpa_lidar,strOut,4+sizeof(point_cloud.header.frame_id.size()) + 4+8+point_cloud.width * sizeof(pcl::PointXYZI));
+    delete strOut;
+ //   pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
+  //  int scan_points_num = filtered_scan_ptr->size();
+}
+
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "rostopilot");
+	ros::NodeHandle n;
+	ros::Rate loop_rate(100);
+
+	    ros::NodeHandle private_nh("~");
+    private_nh.getParam("points_node",_point_topic);
+	private_nh.getParam("points_msgname",_point_msgname);
+    std::cout<<"_point_topic is "<<_point_topic<<std::endl;
+	std::cout<<"_point_msgname : "<<_point_msgname<<std::endl;
+
+	gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(),20000000,1);
+
+   points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
+
+
+	while (ros::ok())
+	{
+ //       ROS_INFO("hello");
+        ros::spinOnce();
+	 	loop_rate.sleep();
+	}
+}