first commit

CMakeLists.txt

@@ -0,0 +1,217 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(motor_control)
+
+# Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+## 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
+        rospy
+        sensor_msgs
+        std_msgs
+        )
+find_package(PCL REQUIRED)
+
+include_directories(include ${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## 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()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   sensor_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 your 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 motor_control
+        #  CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs
+        #  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+        # include
+        ${catkin_INCLUDE_DIRS}
+)
+
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/motor_control.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# 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/motor_control_node.cpp src/motor.cpp src/pid.cpp)
+add_executable(${PROJECT_NAME}_pcd_to_las src/pcd_to_las.cpp)
+# 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 "")
+
+# Add cmake target dependencies of the executable
+# same as for the library above
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_pcd_to_las ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+        ${catkin_LIBRARIES}
+        )
+target_link_libraries(${PROJECT_NAME}_pcd_to_las
+        ${catkin_LIBRARIES}
+        ${PCL_LIBRARIES}
+        /usr/local/lib/liblas.so.3
+        )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_motor_control.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

launch/run.launch

@@ -0,0 +1,5 @@
+<launch>
+    <node pkg="motor_control" type="node" name="motor_control" output="screen">
+        <param name="speed" value="0.1"/>
+    </node>
+</launch>

package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>motor_control</name>
+  <version>0.0.0</version>
+  <description>The motor_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="lenotary@todo.todo">lenotary</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/motor_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, 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 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>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/motor.cpp

@@ -0,0 +1,136 @@
+/**
+ * \file motor.cpp
+ * \brief 简要介绍
+ * \details 详细说明
+ * \author lenotary
+ * \version 1.0
+ * \date 2021/9/13
+ * \pre 先决条件。（有的话才添加。）
+ * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
+ * \warning 警告。（有的话才添加。）
+ * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
+ * \since 一些历史情况记录。（有的话才添加。）
+ */
+
+//
+// Created by lenotary on 2021/9/13.
+//
+
+#include "motor.h"
+
+Motor::Motor(ros::NodeHandle &nh, ros::NodeHandle &privateNh) {
+    m_iPid_ = new Pid(150.0, 2.0, 0.0, 2000, 2000);
+    m_iPubPose_ = nh.advertise<std_msgs::Float64>("motor_pose", 1);
+    m_iPubPoseImu_ = nh.advertise<sensor_msgs::Imu>("motor_pose_imu", 1);
+    m_iPubSpeed_ = nh.advertise<std_msgs::Float64>("motor_speed", 1);
+    m_iPubCurrent_ = nh.advertise<std_msgs::Float64>("motor_current", 1);
+    m_iSubStart_ = nh.subscribe("start_motor", 1, &Motor::setSpeedCallback, this);
+    m_iSubPose_ = nh.subscribe("motor_set_pose", 1, &Motor::setPoseCallback, this);
+    privateNh.getParam("speed", m_dParamSpeed);
+    struct sockaddr_can iAddress{};
+    struct ifreq ifr{};
+    struct can_filter rFilter{};
+    m_nS_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);//创建套接字
+    strcpy(ifr.ifr_name, "can0");
+    ioctl(m_nS_, SIOCGIFINDEX, &ifr); //指定 can0 设备
+    iAddress.can_family = AF_CAN;
+    iAddress.can_ifindex = ifr.ifr_ifindex;
+    bind(m_nS_, (struct sockaddr *) &iAddress, sizeof(iAddress));//将套接字与 can0 绑定
+    setsockopt(m_nS_, SOL_CAN_RAW, CAN_RAW_FILTER, &rFilter, sizeof(rFilter));
+    std::thread t(&Motor::receiveProcess, this);
+    t.detach();
+}
+
+
+void Motor::receiveProcess() {
+    uint64_t count{0};
+    while (ros::ok()) {
+        m_iSpeedMutex_.lock();
+        m_nBytes_ = read(m_nS_, &m_iGetFrame_, sizeof(m_iGetFrame_));
+        if (m_nBytes_ > 0) {
+            m_iMotorInfo_.m_nMotorId = m_iGetFrame_.can_id;
+            m_iMotorInfo_.GetPose((m_iGetFrame_.data[0] << 8) + m_iGetFrame_.data[1]);
+            m_iMotorInfo_.GetSpeed((m_iGetFrame_.data[2] << 8) + m_iGetFrame_.data[3]);
+            m_iMotorInfo_.GetTorqueCurrent((m_iGetFrame_.data[4] << 8) + m_iGetFrame_.data[5]);
+            m_iMotorInfo_.GetTemperature(m_iGetFrame_.data[6]);
+        }
+        static double prevAngle = m_iMotorInfo_.m_dPoseAngle;
+        static double prevTime = ros::Time::now().toSec();
+        if (std::abs(prevAngle - m_iMotorInfo_.m_dPoseAngle) > M_PI and
+            std::abs(prevTime - ros::Time::now().toSec()) > 1.0) {
+            ROS_WARN("info %f %f", prevAngle, m_iMotorInfo_.m_dPoseAngle);
+            m_dSetSpeed = -m_dSetSpeed;
+            prevTime = ros::Time::now().toSec();
+        }
+        prevAngle = m_iMotorInfo_.m_dPoseAngle;
+
+        if( std::abs(m_dSetPose_) > M_PI / 2.0 ){
+            if(m_dSetPose_ < 0){
+                m_dSetPose_ = 2 * M_PI + m_dSetPose_;
+            }
+            m_iMotorInfo_.m_nSetVoltage = m_iPid_->PidCalc(m_iMotorInfo_.poseRadToInt(m_dSetPose_),
+                                                           m_iMotorInfo_.m_nPoseInt2);
+//            ROS_INFO("info ss %d %d",m_iMotorInfo_.poseRadToInt(m_dSetPose_),m_iMotorInfo_.m_nPoseInt2);
+        }else{
+            m_iMotorInfo_.m_nSetVoltage = m_iPid_->PidCalc(m_iMotorInfo_.poseRadToInt(m_dSetPose_),
+                                                           m_iMotorInfo_.m_nPoseInt);
+//            ROS_INFO("info ss %d %d",m_iMotorInfo_.poseRadToInt(m_dSetPose_),m_iMotorInfo_.m_nPoseInt);
+        }
+
+        sendMsg(1, static_cast<short >( m_iMotorInfo_.m_nSetVoltage));
+        std_msgs::Float64 msg;
+        msg.data = m_iMotorInfo_.m_dPoseAngle;
+        m_iPubPose_.publish(msg);
+        msg.data = m_iMotorInfo_.m_dSpeedRad;
+        m_iPubSpeed_.publish(msg);
+        msg.data = m_iMotorInfo_.m_dTorqueCurrent;
+        m_iPubCurrent_.publish(msg);
+
+        sensor_msgs::Imu imuMsg;
+        imuMsg.header.seq = count++;
+        imuMsg.header.stamp = ros::Time::now();
+        imuMsg.header.frame_id = "motor";
+        auto q = tf::createQuaternionFromYaw(m_iMotorInfo_.m_dPoseAngle);
+        imuMsg.orientation.x = q.x();
+        imuMsg.orientation.y = q.y();
+        imuMsg.orientation.z = q.z();
+        imuMsg.orientation.w = q.w();
+        m_iPubPoseImu_.publish(imuMsg);
+        m_iSpeedMutex_.unlock();
+    }
+}
+
+void Motor::sendMsg(uint32_t id, short data) {
+    struct can_frame sendFrame{};
+    sendFrame.can_id = 0x1ff;
+    sendFrame.can_dlc = 8;
+    sendFrame.data[0] = (data >> 8) & 0xff;
+    sendFrame.data[1] = (data) & 0xff;
+
+    m_nBytes_ = write(m_nS_, &sendFrame, sizeof(sendFrame));
+    if (m_nBytes_ != sizeof(sendFrame)) {
+        ROS_WARN("send error");
+    }
+}
+
+void Motor::setSpeedCallback(const std_msgs::Bool::ConstPtr &msg) {
+    m_iSpeedMutex_.lock();
+    m_bIsRun_ = msg->data;
+//    if(msg->data == true){
+//        m_dSetSpeed = m_dParamSpeed;
+//    }else{
+//        m_dSetSpeed = 0;
+//    }
+    m_iSpeedMutex_.unlock();
+}
+void Motor::setPoseCallback(const std_msgs::Float64::ConstPtr &msg) {
+    m_iSpeedMutex_.lock();
+    ROS_INFO("[motor] get pose %f",msg->data);
+    m_dSetPose_ = msg->data;
+    if(m_dSetPose_ > M_PI){
+        m_dSetPose_ = M_PI;
+    }else if(m_dSetPose_ < -M_PI){
+        m_dSetPose_ = -M_PI;
+    }
+    m_iSpeedMutex_.unlock();
+}

src/motor.h

@@ -0,0 +1,126 @@
+/**
+ * \file motor.h
+ * \brief 简要介绍
+ * \details 详细说明
+ * \author lenotary
+ * \version 1.0
+ * \date 2021/9/13
+ * \pre 先决条件。（有的话才添加。）
+ * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
+ * \warning 警告。（有的话才添加。）
+ * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
+ * \since 一些历史情况记录。（有的话才添加。）
+ */
+
+
+//
+// Created by lenotary on 2021/9/13.
+//
+
+#ifndef SRC_MOTOR_H
+#define SRC_MOTOR_H
+
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <tf2/transform_datatypes.h>
+#include <tf/tf.h>
+#include <linux/can.h>
+#include <cstdlib>
+#include <cstdio>
+#include <cstring>
+#include <unistd.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <linux/can/raw.h>
+#include <thread>
+#include <cmath>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Bool.h>
+#include <sensor_msgs/Imu.h>
+#include <mutex>
+#include "pid.h"
+
+
+class MotorInfo {
+public:
+    uint32_t m_nMotorId;
+    double m_nSetVoltage;
+    short m_nSetSpeedInt;
+    short m_nSpeedInt;
+    int m_nPoseInt;
+    int m_nPoseInt2;
+    short m_nTorqueCurrentInt;
+    short m_nTemperatureInt;
+    double m_dSpeedRad;
+    double m_dPoseAngle;
+    double m_dTorqueCurrent;
+
+    short speedRadToInt(double input) {
+        return static_cast<short>(input * 60.0 / M_PI);
+    }
+    short poseRadToInt(double input) {
+        return static_cast<short>(input * 8192.0 / 2 / M_PI);
+    }
+
+    void GetSpeed(short input) {
+        m_nSpeedInt = input;
+        m_dSpeedRad = m_nSpeedInt / 60.0 * M_PI;
+    }
+
+    void GetPose(short input) {
+        m_nPoseInt = input;
+        m_nPoseInt2 = input;
+        if(input > 8192 / 2){
+            m_nPoseInt = input - 8192;
+        }
+
+        m_dPoseAngle = input / 8192.0 * 2 * M_PI;
+        if (m_dPoseAngle > M_PI) {
+            m_dPoseAngle = m_dPoseAngle - 2 * M_PI;
+        }
+    }
+
+    void GetTorqueCurrent(short input) {
+        m_nTorqueCurrentInt = input;
+        m_dTorqueCurrent = input / 1000.0;
+    }
+
+    void GetTemperature(short input) {
+        m_nTemperatureInt = input;
+    }
+
+    void SetSpeed(double input) {
+        m_nSetSpeedInt = static_cast<short >(input * 60.0 / M_PI);
+    }
+
+};
+
+class Motor {
+public:
+    Motor(ros::NodeHandle &nh, ros::NodeHandle &privateNh);
+
+private:
+    int m_nS_, m_nBytes_;
+    ros::Publisher m_iPubPoseImu_;
+    ros::Publisher m_iPubPose_;
+    ros::Publisher m_iPubSpeed_;
+    ros::Publisher m_iPubCurrent_;
+    ros::Subscriber m_iSubStart_;
+    ros::Subscriber m_iSubPose_;
+    Pid *m_iPid_;
+    MotorInfo m_iMotorInfo_;
+    std::mutex m_iSpeedMutex_;
+    struct canfd_frame m_iGetFrame_{};
+    double m_dSetPose_{0.0};
+    bool m_bIsRun_;
+    double m_dSetSpeed{0};
+    double m_dParamSpeed{0};
+    void receiveProcess();
+    void sendMsg(uint32_t id, short data);
+    void setSpeedCallback(const std_msgs::Bool::ConstPtr &msg);
+    void setPoseCallback(const std_msgs::Float64::ConstPtr &msg);
+};
+
+
+#endif //SRC_MOTOR_H

src/motor_control_node.cpp

@@ -0,0 +1,27 @@
+/**
+ * \file motor_control_node.cpp
+ * \brief 简要介绍
+ * \details 详细说明
+ * \author lenotary
+ * \version 1.0
+ * \date 2021/9/13
+ * \pre 先决条件。（有的话才添加。）
+ * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
+ * \warning 警告。（有的话才添加。）
+ * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
+ * \since 一些历史情况记录。（有的话才添加。）
+ */
+//
+// Created by lenotary on 2021/9/13.
+//
+#include "motor.h"
+int main(int argc, char **argv) {
+    setlocale(LC_CTYPE,"zh_CN.utf8");
+    ros::init(argc,argv,"motor_node");
+    ros::NodeHandle nh;
+    ros::NodeHandle privateNh("~");
+    Motor motor(nh,privateNh);
+    ROS_INFO("你好 世界");
+    ros::spin();
+    return 0;
+}

src/pcd_to_las.cpp

@@ -0,0 +1,159 @@
+/**
+ * \file pcd_to_las.cpp
+ * \brief 简要介绍
+ * \details 详细说明
+ * \author lenotary
+ * \version 1.0
+ * \date 2021/12/16
+ * \pre 先决条件。（有的话才添加。）
+ * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
+ * \warning 警告。（有的话才添加。）
+ * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
+ * \since 一些历史情况记录。（有的话才添加。）
+ */
+//
+// Created by lenotary on 2021/12/16.
+//
+
+
+#include <liblas/liblas.hpp>
+#include <iomanip>
+#include <iostream>
+#include <sstream>
+#include <cmath>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <string>
+
+//  实现Unshort转为Unchar类型
+//int Unshort2Unchar(uint16_t &green, uint8_t &g)
+//{
+//    unsigned short * word;
+//    word = &green;
+//    int size = WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, NULL, 0, NULL, FALSE);
+//    char *AsciiBuff=new char[size];
+//    WideCharToMultiByte(CP_ACP, 0, LPCWSTR(word), -1, AsciiBuff, size, NULL, FALSE);
+//    g = *AsciiBuff;
+//
+//    delete[] AsciiBuff;
+//    AsciiBuff = NULL;
+//    return 0;
+//}
+
+void writePointCloudFromLas(const char* strInputLasName, const char* strOutPutPointCloudName)
+{
+    //打开las文件
+    std::ifstream ifs;
+    ifs.open(strInputLasName, std::ios::in | std::ios::binary);
+    if (!ifs.is_open())
+    {
+        std::cout << "无法打开.las" << std::endl;
+        return;
+    }
+    liblas::ReaderFactory readerFactory;
+    liblas::Reader reader = readerFactory.CreateWithStream(ifs);
+    //写点云
+    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOutput(new pcl::PointCloud<pcl::PointXYZRGBA>());
+    cloudOutput->clear();
+    while (reader.ReadNextPoint())
+    {
+        double x = reader.GetPoint().GetX();
+        double y = reader.GetPoint().GetY();
+        double z = reader.GetPoint().GetZ();
+        uint16_t red = reader.GetPoint().GetColor()[0];
+        uint16_t green = reader.GetPoint().GetColor()[1];
+        uint16_t blue = reader.GetPoint().GetColor()[2];
+
+        /*****颜色说明
+        *   uint16_t  范围为0-256*256 ；
+        *   pcl 中 R  G  B利用的unsigned char  0-256；
+        *   因此这里将uint16_t 除以256  得到  三位数的0-256的值
+        *   从而进行rgba 值32位 的位运算。
+        *
+        ******/
+
+        pcl::PointXYZRGBA thePt;  //int rgba = 255<<24 | ((int)r) << 16 | ((int)g) << 8 | ((int)b);
+        thePt.x = x; thePt.y = y; thePt.z = z;
+        thePt.rgba = (uint32_t)255 << 24 | (uint32_t)(red / 256) << 16 | (uint32_t)(green / 256) << 8 | (uint32_t)(blue / 256);
+        //uint32_t rgb = *reinterpret_cast<int*>(&thePt.rgb);  //reinterpret_cast 强制转换
+        cloudOutput->push_back(thePt);
+    }
+
+    //boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
+    //viewer->setBackgroundColor(0, 0, 0); //设置背景
+    //viewer->addPointCloud(cloudOutput,"cloudssd");
+    //while (!viewer->wasStopped()){
+    //    viewer->spinOnce();
+    //}
+    pcl::io::savePCDFileASCII(strOutPutPointCloudName, *cloudOutput);
+    cloudOutput->clear();
+}
+
+//读入点云，写.las
+
+void writeLasFromPointCloud3(const std::string strInputPointCloudName, std::string strOutLasName)
+{
+    std::ofstream ofs(strOutLasName.c_str(), ios::out | ios::binary);
+    if (!ofs.is_open())
+    {
+        std::cout << "err  to  open  file  las....." << std::endl;
+        return;
+    }
+    liblas::Header header;
+    header.SetVersionMajor(1);
+    header.SetVersionMinor(2);
+    header.SetDataFormatId(liblas::ePointFormat3);
+    header.SetScale(0.001, 0.001, 0.001);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::io::loadPCDFile(strInputPointCloudName, *cloud);
+    std::cout << "总数:" << cloud->points.size() << std::endl;
+    //写liblas,
+    liblas::Writer writer(ofs, header);
+    liblas::Point point(&header);
+
+    for (size_t i = 0; i < cloud->points.size(); i++)
+    {
+        double x = cloud->points[i].x;
+        double y = cloud->points[i].y;
+        double z = cloud->points[i].z;
+        point.SetCoordinates(x, y, z);
+
+//        uint32_t red = (uint32_t)cloud->points[i].r;
+//        uint32_t green = (uint32_t)cloud->points[i].g;
+//        uint32_t blue = (uint32_t)cloud->points[i].b;
+//        liblas::Color pointColor(red, green, blue);
+//        point.SetColor(pointColor);
+        writer.WritePoint(point);
+        //std::cout << x << "," << y << "," << z << std::endl;
+    }
+    double minPt[3] = { 9999999, 9999999, 9999999 };
+    double maxPt[3] = { 0, 0, 0 };
+    header.SetPointRecordsCount(cloud->points.size());
+    header.SetPointRecordsByReturnCount(0, cloud->points.size());
+    header.SetMax(maxPt[0], maxPt[1], maxPt[2]);
+    header.SetMin(minPt[0], minPt[1], minPt[2]);
+    writer.SetHeader(header);
+}
+
+int main(int argc,char** argv)
+{
+    //char strInputLasName[] = "qq.las";//"Scan_az001.las";
+    //char strOutPutPointCloudName[]="qqqq.pcd";
+    //writePointCloudFromLas(strInputLasName, strOutPutPointCloudName);
+
+    //std::string strInputPointCloudName = "eewge";
+    //std::string strOutLasName = "eewge";
+    //writeLasFromPointCloud(strInputPointCloudName.c_str(), strOutLasName.c_str());
+    if(argc<3){std::cout<<"参数个数不能少于三个！"<<std::endl;
+        return -1;}
+
+    std::string strInputPointCloudName = argv[1];
+    std::string strOutLasName = argv[2];
+    writeLasFromPointCloud3(strInputPointCloudName, strOutLasName);
+
+    return 0;
+}

src/pid.cpp

@@ -0,0 +1,44 @@
+/**
+ * \file pid.cpp
+ * \brief 简要介绍
+ * \details 详细说明
+ * \author lenotary
+ * \version 1.0
+ * \date 2021/9/13
+ * \pre 先决条件。（有的话才添加。）
+ * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
+ * \warning 警告。（有的话才添加。）
+ * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
+ * \since 一些历史情况记录。（有的话才添加。）
+ */
+
+//
+// Created by lenotary on 2021/9/13.
+//
+
+#include "pid.h"
+
+Pid::Pid(double kp, double ki, double kd, double iMax, double outMax) {
+    m_fKp_ = kp;
+    m_fKi_ = ki;
+    m_fKd_ = kd;
+    m_fInMax_ = iMax;
+    m_fOutMax = outMax;
+}
+
+double Pid::PidCalc(int ref, int fdb) {
+    m_dRef_ = ref;
+    m_dFdb_ = fdb;
+    m_dErr_[1] = m_dErr_[0];
+    m_dErr_[0] = ref - fdb;
+    m_dPOut_ = m_fKp_ * m_dErr_[0];
+
+    m_dIOut_ += m_fKi_ * m_dErr_[0];
+    m_dDOut_ = m_fKd_ * (m_dErr_[0] - m_dErr_[1]);
+    LIMIT_MIN_MAX(m_dIOut_, -m_fInMax_, m_fInMax_);
+    m_DOutput_ = m_dPOut_ + m_dIOut_ + m_dDOut_;
+    LIMIT_MIN_MAX(m_DOutput_, -m_fOutMax, m_fOutMax);
+
+    return m_DOutput_;
+}
+

src/pid.h

@@ -0,0 +1,50 @@
+/**
+ * \file pid.h
+ * \brief 简要介绍
+ * \details 详细说明
+ * \author lenotary
+ * \version 1.0
+ * \date 2021/9/13
+ * \pre 先决条件。（有的话才添加。）
+ * \bug 存在的bug。（有的话才添加，注明“还未发现”也可以。）
+ * \warning 警告。（有的话才添加。）
+ * \copyright 版权声明。（通常只写遵循什么协议，版权详细内容应放在LICENSE文件中，并且应该与LICENSE文件的内容统一，不能自相矛盾。）
+ * \since 一些历史情况记录。（有的话才添加。）
+ */
+
+
+//
+// Created by lenotary on 2021/9/13.
+//
+
+#ifndef SRC_PID_H
+#define SRC_PID_H
+#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
+#include <iostream>
+
+class Pid {
+public:
+    Pid(double kp, double ki, double kd, double iMax, double outMax);
+
+    Pid();
+
+    double PidCalc(int ref, int fdb);
+private:
+    double m_fKp_;
+    double m_fKi_;
+    double m_fKd_;
+    double m_fInMax_;
+    double m_fOutMax;
+
+    double m_dRef_{};      // target value
+    double m_dFdb_{};      // feedback value
+    double m_dErr_[2]{};   // error and last error
+
+    double m_dPOut_{};
+    double m_dIOut_{};
+    double m_dDOut_{};
+    double m_DOutput_{};
+};
+
+
+#endif //SRC_PID_H