From 06c0b0381171a0e4ad53d176f775754839519aa6 Mon Sep 17 00:00:00 2001
From: hermanoid <lucasniewohner@gmail.com>
Date: Wed, 29 Apr 2020 10:27:40 -0500
Subject: [PATCH 1/2] Add simple param- and service-based speed control

---
 CMakeLists.txt     |  8 ++++++++
 package.xml        |  2 ++
 src/node.cpp       | 22 +++++++++++++++++++++-
 srv/MotorSpeed.srv |  2 ++
 4 files changed, 33 insertions(+), 1 deletion(-)
 create mode 100644 srv/MotorSpeed.srv

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a5f22489..481f4f08 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,8 +13,16 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rosconsole
   sensor_msgs
+  message_generation
 )
 
+add_service_files(
+  FILES
+  MotorSpeed.srv
+)
+
+generate_messages()
+
 include_directories(
   ${RPLIDAR_SDK_PATH}/include
   ${RPLIDAR_SDK_PATH}/src
diff --git a/package.xml b/package.xml
index 78c1a295..183f0a88 100644
--- a/package.xml
+++ b/package.xml
@@ -16,5 +16,7 @@
   <run_depend>rosconsole</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
 
 </package>
diff --git a/src/node.cpp b/src/node.cpp
index 59837ba2..49de97f7 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -36,6 +36,7 @@
 #include "sensor_msgs/LaserScan.h"
 #include "std_srvs/Empty.h"
 #include "rplidar.h"
+#include "rplidar_ros/MotorSpeed.h"
 
 #ifndef _countof
 #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
@@ -177,6 +178,20 @@ bool start_motor(std_srvs::Empty::Request &req,
   return true;
 }
 
+bool set_motor_speed(rplidar_ros::MotorSpeed::Request &req,
+                               rplidar_ros::MotorSpeed::Response &res)
+{
+  if(!drv)
+       return false;
+  if(drv->isConnected())
+  {
+      ROS_DEBUG("Set Motor PWM");
+      u_result ans=drv->setMotorPWM(req.speed);
+   }
+   else ROS_INFO("lost connection");
+  return true;
+}
+
 static float getAngle(const rplidar_response_measurement_node_hq_t& node)
 {
     return node.angle_z_q14 * 90.f / 16384.f;
@@ -196,6 +211,7 @@ int main(int argc, char * argv[]) {
     float max_distance = 8.0;
     int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree
     std::string scan_mode;
+    int motor_speed;
     ros::NodeHandle nh;
     ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
     ros::NodeHandle nh_private("~");
@@ -208,7 +224,8 @@ int main(int argc, char * argv[]) {
     nh_private.param<bool>("inverted", inverted, false);
     nh_private.param<bool>("angle_compensate", angle_compensate, false);
     nh_private.param<std::string>("scan_mode", scan_mode, std::string());
-
+    nh_private.param<int>("motor_speed", motor_speed, DEFAULT_MOTOR_PWM);
+    
     ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");
 
     u_result     op_result;
@@ -259,9 +276,12 @@ int main(int argc, char * argv[]) {
 
     ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);
     ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor);
+    ros::ServiceServer set_motor_speed_service = nh.advertiseService("set_motor_speed", set_motor_speed);
 
     drv->startMotor();
 
+    drv->setMotorPWM(motor_speed);
+
     RplidarScanMode current_scan_mode;
     if (scan_mode.empty()) {
         op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, &current_scan_mode);
diff --git a/srv/MotorSpeed.srv b/srv/MotorSpeed.srv
new file mode 100644
index 00000000..1b6255b1
--- /dev/null
+++ b/srv/MotorSpeed.srv
@@ -0,0 +1,2 @@
+uint16 speed
+---

From 4bfbd9ca4a539733b16a2069d50ac40b925f2310 Mon Sep 17 00:00:00 2001
From: Lucas <lucasniewohner@gmail.com>
Date: Thu, 14 May 2020 10:34:00 -0500
Subject: [PATCH 2/2] Fixed a build issue from my previous commit

---
 CMakeLists.txt | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 481f4f08..d81a9916 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,7 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-catkin_package()
+catkin_package(CATKIN_DEPENDS message_runtime)
 
 add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC})
 target_link_libraries(rplidarNode ${catkin_LIBRARIES})
@@ -37,6 +37,8 @@ target_link_libraries(rplidarNode ${catkin_LIBRARIES})
 add_executable(rplidarNodeClient src/client.cpp)
 target_link_libraries(rplidarNodeClient ${catkin_LIBRARIES})
 
+add_dependencies(rplidarNode ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 install(TARGETS rplidarNode rplidarNodeClient
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}