Merge pull request #21 from WHILL/christmas2019

Preparing for Christmas Gift 2019!

README.md

@@ -15,6 +15,9 @@ For general questions and requests, please visit https://whill.zendesk.com/hc/ja
 #### ~controller/joy [(sensor_msgs/Joy)](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html)
 - Virtual WHILL joystick input. You can controll WHILL via this topic.
 
+#### ~controller/cmd_vel [(geometry_msgs/Twist)](http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html)
+- cmc_vel input. You can controll WHILL via this topic.
+- This command is only available Model CR firmware updatedd after 2019.12. If you want to use this cmd_vel, please update firmware of Model CR by contact to sales of WHILL.
 
 ### Published Topics
 

src/ros_whill.cpp

@@ -268,6 +268,20 @@ void sleep_ms(uint32_t ms){
 //
 // WHILL
 //
+
+// Enable cmd_vel Topic
+bool enable_cmd_vel_topic = false;
+ros::Subscriber cmd_vel_subscriber;
+void activate_cmd_vel_topic(ros::NodeHandle &nh)
+{
+    static bool activated = false;
+    if (!activated && enable_cmd_vel_topic)
+    {
+        cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback);
+        activated = true;
+    }
+}
+
 void whill_callback_data1(WHILL *caller)
 {
 
@@ -358,6 +372,7 @@ void whill_callback_data1(WHILL *caller)
     }
     else if (caller->_interval >= 0)
     {
+        enable_cmd_vel_topic = true;
         // Experimental
         if(caller->_interval == 0){
             odom.zeroVelocity();
@@ -440,20 +455,10 @@ int main(int argc, char **argv)
     // Disable publishing odometry tf
     nh.param<bool>("publish_tf", publish_tf, true);
 
-    // Enable Experimantal Topics
-    bool experimental_topics;
-    nh.param<bool>("experimental_topics", experimental_topics, false);
 
     bool keep_connected;
     nh.param<bool>("keep_connected", keep_connected, false);
 
-    ros::Subscriber cmd_vel_subscriber;
-    if (experimental_topics == true)
-    {
-        ROS_INFO("Experimental topics activated");
-        cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback);
-    }
-
     unsigned long baud = 38400;
     serial::Timeout timeout = serial::Timeout::simpleTimeout(0);
     timeout.write_timeout_multiplier = 5; // Wait 5ms every bytes
@@ -527,6 +532,7 @@ int main(int argc, char **argv)
         while (ros::ok())
         {
             whill->refresh();
+            activate_cmd_vel_topic(nh);
             rate.sleep();
             if (keep_connected && (abs((last_received - ros::Time::now()).toSec()) > 2.0))
             {

src/whill/whill_commands.cpp

@@ -114,37 +114,23 @@ void WHILL::setBatteryVoltaegeOut(bool enable){
 // Experimental, Speed control without Jerk control but only Acceleration cotnrol.
 void WHILL::setSpeed(float linear,  float angular)
 {
-    int x, y;
+    int16_t x = 0, y = 0; // Unit: 0.004km/h
 
-    const float front_max = 6.0;     // [km/h]
-    const float spin_max = 3.5;      // [km/h]
-    const float back_max = 2.0;      // [km/h]
-    const float tread_width = 0.248; // [m]
-
-    float desire_front_kmh = linear * 3.6;                    // [m/s]   to [km/h]
-    float desire_spin_spd = -tread_width * angular * 3.6 * 2; // [rad/s] to [km/h]
-
-    if (linear >= 0)
-    {
-        //forward
-        y = desire_front_kmh / front_max * 100;
-        y = y > 100 ? 100 : y;
-    }
-    else
-    {
-        //back
-        y = desire_front_kmh / back_max * 100;
-        y = y < -100 ? -100 : y;
-    }
+    // Linear
+    y = (linear * 3.6f) / 0.004f;
 
-    x = desire_spin_spd / spin_max * 100;
-    x = x > 100 ? 100 : x;
-    x = x < -100 ? -100 : x;
-
-    unsigned char payload[] = {0x08, // Experimental Command, Control with Low Jerk, Almost Const-Accel control
-                               0x00, 
-                               (unsigned char)(char)(y),
-                               (unsigned char)(char)(x)};
+    // angular
+    const float tread_width = 0.248; // [m]
+    float v = tread_width * angular;  // V=rω [m/s]
+    x = -v * 3.6f / 0.004f * 2.0f; // [m/s] to [km/h]→ 0.004km/h unit, Double for differencial
+
+    unsigned char payload[] = {
+        0x08, // Experimental Command, Control with Low Jerk, Almost Const-Accel control
+        0,    // 0=Disable Joystick 1=Allow Joystick
+        (uint8_t)((y >> 8) & 0xff),
+        (uint8_t)((y >> 0) & 0xff),
+        (uint8_t)((x >> 8) & 0xff),
+        (uint8_t)((x >> 0) & 0xff)};
     Packet packet(payload, sizeof(payload));
     packet.build();
     transferPacket(&packet);