Motor PWM as a node and service parameter

CMakeLists.txt

@@ -13,6 +13,17 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rosconsole
   sensor_msgs
+  message_generation
+)
+
+add_service_files(
+  FILES
+  StartMotor.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
 )
 
 include_directories(
@@ -24,6 +35,7 @@ include_directories(
 catkin_package()
 
 add_executable(rplidarNode src/node.cpp ${RPLIDAR_SDK_SRC})
+add_dependencies(rplidarNode ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(rplidarNode ${catkin_LIBRARIES})
 
 add_executable(rplidarNodeClient src/client.cpp)

package.xml

@@ -12,9 +12,11 @@
   <build_depend>rosconsole</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
+  <build_depend>message_generation</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rosconsole</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
+  <run_depend>message_runtime</run_depend>
 
 </package>

sdk/include/rplidar_driver.h

@@ -117,7 +117,7 @@ class RPlidarDriver {
     virtual u_result setMotorPWM(_u16 pwm) = 0;
 
     /// Start RPLIDAR's motor when using accessory board
-    virtual u_result startMotor() = 0;
+    virtual u_result startMotor(_u16 pwm) = 0;
 
     /// Stop RPLIDAR's motor when using accessory board
     virtual u_result stopMotor() = 0;

sdk/src/rplidar_driver.cpp

@@ -982,6 +982,11 @@ u_result RPlidarDriverSerialImpl::setMotorPWM(_u16 pwm)
 {
     u_result ans;
     rplidar_payload_motor_pwm_t motor_pwm;
+
+    //protection:
+    if(pwm > MAX_MOTOR_PWM)
+        pwm = MAX_MOTOR_PWM;
+
     motor_pwm.pwm_value = pwm;
 
     {
@@ -995,10 +1000,10 @@ u_result RPlidarDriverSerialImpl::setMotorPWM(_u16 pwm)
     return RESULT_OK;
 }
 
-u_result RPlidarDriverSerialImpl::startMotor()
+u_result RPlidarDriverSerialImpl::startMotor(_u16 pwm)
 {
     if (_isSupportingMotorCtrl) { // RPLIDAR A2
-        setMotorPWM(DEFAULT_MOTOR_PWM);
+        setMotorPWM(pwm);
         delay(500);
         return RESULT_OK;
     } else { // RPLIDAR A1

sdk/src/rplidar_driver_serial.h

@@ -63,7 +63,7 @@ class RPlidarDriverSerialImpl : public RPlidarDriver
     virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT);
 
     virtual u_result setMotorPWM(_u16 pwm);
-    virtual u_result startMotor();
+    virtual u_result startMotor(_u16 pwm);
     virtual u_result stopMotor();
     virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT);
 	virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode);

src/node.cpp

@@ -35,6 +35,7 @@
 #include "ros/ros.h"
 #include "sensor_msgs/LaserScan.h"
 #include "std_srvs/Empty.h"
+#include "rplidar_ros/StartMotor.h"
 #include "rplidar.h"
 
 #ifndef _countof
@@ -169,14 +170,14 @@ bool stop_motor(std_srvs::Empty::Request &req,
   return true;
 }
 
-bool start_motor(std_srvs::Empty::Request &req,
-                               std_srvs::Empty::Response &res)
+bool start_motor(rplidar_ros::StartMotor::Request &req,
+                               rplidar_ros::StartMotor::Response &res)
 {
   if(!drv)
        return false;
-  ROS_DEBUG("Start motor");
-  drv->startMotor();
-  drv->startScan();;
+  ROS_DEBUG("Start motor (%d)", req.pwm.data);
+  drv->startMotor(req.pwm.data);
+  drv->startScan();
   return true;
 }
 
@@ -185,6 +186,7 @@ int main(int argc, char * argv[]) {
 
     std::string serial_port;
     int serial_baudrate = 115200;
+    int motor_pwm;
     std::string frame_id;
     bool inverted = false;
     bool angle_compensate = true;
@@ -194,6 +196,7 @@ int main(int argc, char * argv[]) {
     ros::NodeHandle nh_private("~");
     nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); 
     nh_private.param<int>("serial_baudrate", serial_baudrate, 115200); 
+    nh_private.param<int>("motor_pwm", motor_pwm, DEFAULT_MOTOR_PWM);
     nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
     nh_private.param<bool>("inverted", inverted, false);
     nh_private.param<bool>("angle_compensate", angle_compensate, true);
@@ -233,7 +236,7 @@ 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);
 
-    drv->startMotor();
+    drv->startMotor(motor_pwm);
     drv->startScan();
 
     ros::Time start_scan_time;

srv/StartMotor.srv

@@ -0,0 +1,2 @@
+std_msgs/UInt16 pwm
+---