twist_to_manual_control: added taking into account zero value params in speed/steering ranges

cfg/TwistToManualControl.cfg

@@ -39,18 +39,20 @@ gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
-gen.add("steering_min",  int_t,  0,  "Minimum steering value",  45,      0,  180)
-gen.add("steering_max",  int_t,  0,  "Maximum steering value",  135,      0,  180)
-gen.add("angular_sat_min",  double_t,  0,  "Maximum angular speed value to take into account",  -0.5,      -5.0,  5.0)
-gen.add("angular_sat_max",  double_t,  0,  "Minimum angular speed value to take into account",  0.5,      -5.0,  5.0)
-
-gen.add("speed_min",  int_t,  0,  "Minimum speed value",  -500,      -1000,  1000)
-gen.add("speed_max",  int_t,  0,  "Maximum speed value",  500,      -1000,  1000)
-gen.add("linear_sat_min",  double_t,  0,  "Maximum linear speed value to take into account",  -0.5,      -5.0,  5.0)
-gen.add("linear_sat_max",  double_t,  0,  "Minimum linear speed value to take into account",  0.5,      -5.0,  5.0)
-
-gen.add("invert_speed",  bool_t,  0,  "Invert speed signum",  False)
-gen.add("invert_steering",  bool_t,  0,  "Invert steering direction",  False)
+gen.add("steering_min",  int_t, 0, "Minimum output steering value",  45, 0, 180)
+gen.add("steering_zero", int_t, 0, "Zero output steering value",     90, 0, 180)
+gen.add("steering_max",  int_t, 0, "Maximum output steering value", 135, 0, 180)
+gen.add("angular_sat_min", double_t, 0, "Maximum input angular speed", -0.5, -5.0, 5.0)
+gen.add("angular_sat_max", double_t, 0, "Minimum input angular speed",  0.5, -5.0, 5.0)
+
+gen.add("speed_min",  int_t, 0, "Minimum output speed value", -500, -1000, 1000)
+gen.add("speed_zero", int_t, 0, "Zero output speed value",       0, -1000, 1000)
+gen.add("speed_max",  int_t, 0, "Maximum output speed value",  500, -1000, 1000)
+gen.add("linear_sat_min", double_t, 0, "Maximum input linear speed",  -0.5, -5.0, 5.0)
+gen.add("linear_sat_max", double_t, 0, "Minimum input linear speed ",  0.5, -5.0, 5.0)
+
+gen.add("invert_speed",    bool_t, 0, "Invert speed signum",       False)
+gen.add("invert_steering", bool_t, 0, "Invert steering direction", False)
 
 
 

config/ps3.yaml

@@ -3,7 +3,7 @@ scale_linear: 0.5
 scale_linear_turbo: 2.0
 
 axis_angular: 2
-scale_angular: 0.5
+scale_angular: 1.0
 
 enable_button: 11  # R1 shoulder button
 enable_turbo_button: 9  # R2 shoulder button

include/twist_to_manual_control_alg_node.h

@@ -67,6 +67,8 @@ class TwistToManualControlAlgNode : public algorithm_base::IriBaseAlgorithm<Twis
     // [action server attributes]
 
     // [action client attributes]
+    
+    double clip(double n, double lower, double upper);
 
    /**
     * \brief config variable

launch/node.launch

 <!-- -->
 <launch>
   
-  <arg name="node_name"             default="twist_to_manual_control"/>
+  <arg name="node_name" default="twist_to_manual_control"/>
+  <arg name="invert_speed"    default="True"/>
+  <arg name="invert_steering" default="True"/>
 
 <!--  <group ns="$(optenv ROBOT car)">-->
 
@@ -12,21 +14,23 @@
       <remap from="~cmd_vel"  to="/cmd_vel"/>
       <remap from="~steering" to="/manual_control/steering"/>
       <remap from="~speed"    to="/manual_control/speed"/>
-      
-      <param name="steering_min" value="70"/>
-      <param name="steering_max" value="110"/>
+       
+      <param name="steering_min"  value="20"/>
+      <param name="steering_zero" value="90"/>
+      <param name="steering_max"  value="160"/>
 
-      <param name="angular_sat_min" value="-0.5"/>
-      <param name="angular_sat_max" value="0.5"/>
+      <param name="angular_sat_min" value="-1.0"/>
+      <param name="angular_sat_max" value="1.0"/>
       
-      <param name="speed_min" value="-500"/>
-      <param name="speed_max" value="500"/>
+      <param name="speed_min"  value="-1000"/>
+      <param name="speed_zero" value="0"/>
+      <param name="speed_max"  value="1000"/>
       
       <param name="linear_sat_min" value="-1.0"/>
       <param name="linear_sat_max" value="1.0"/>
       
-      <param name="invert_speed" value="True"/>
-      <param name="invert_steering" value="True"/>
+      <param name="invert_speed"    value="$(arg invert_speed)"/>
+      <param name="invert_steering" value="$(arg invert_steering)"/>
     </node>
 
 <!--  </group>-->

src/twist_to_manual_control_alg_node.cpp

@@ -59,58 +59,87 @@ void TwistToManualControlAlgNode::mainNodeThread(void)
 /*  [subscriber callbacks] */
 void TwistToManualControlAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
 {
-  double min;
-  double max;
-  //ROS_INFO("TwistToManualControlAlgNode::cmd_vel_callback: New Message Received");
+  double min_in, max_in, zero_in;
+  int min_out, max_out, zero_out;
 
-  //use appropiate mutex to shared variables if necessary
-  //this->alg_.lock();
-  //this->cmd_vel_mutex_enter();
-  
-  //  NewValue = (((OldValue - OldMin)*(NewMax - NewMin)) / (OldMax - OldMin)) + NewMin
-  double angular = msg->angular.z;
-  if (angular > this->config_.angular_sat_max)
-    angular=this->config_.angular_sat_max;
-  
-  if (angular < this->config_.angular_sat_min)
-    angular=this->config_.angular_sat_min;
+  //STEERING
+  int steering;
+  double angular;
+
+  if(this->config_.invert_steering)
+    angular = - msg->angular.z;
+  else
+    angular = msg->angular.z;
   
-  if(!this->config_.invert_steering)
+  min_in=this->config_.angular_sat_min;
+  max_in=this->config_.angular_sat_max;
+  zero_in=0.0;
+
+  angular = this->clip(angular, min_in, max_in);
+
+  zero_out = this->config_.steering_zero;
+
+  if(angular>0)
   {
-    min=this->config_.steering_min;
-    max=this->config_.steering_max;
+    min_in  = zero_in;
+    min_out = zero_out;
+    max_out = this->config_.steering_max;
+
   }
-  else
+  else //if(angular<=0)
   {
-    min=this->config_.steering_max;
-    max=this->config_.steering_min;
+    max_in  = zero_in;
+    min_out = this->config_.steering_min;
+    max_out = zero_out;
   }
-  int steering = (((angular  - this->config_.angular_sat_min) * (max - min)) / (this->config_.angular_sat_max - this->config_.angular_sat_min)) + min;
-  
-  double linear = msg->linear.x;
-  if (linear > this->config_.linear_sat_max)
-    linear=this->config_.linear_sat_max;
-  
-  if (linear < this->config_.linear_sat_min)
-    linear=this->config_.linear_sat_min;
-  
-  min=this->config_.speed_min;
-  max=this->config_.speed_max;
-  int speed  = (((linear   - this->config_.linear_sat_min)  * (max - min))    / (this->config_.linear_sat_max  - this->config_.linear_sat_min))  + min;
   
+  steering = (((angular  - min_in) * (max_out - min_out)) / (max_in - min_in)) + min_out;
   
   this->steering_Int16_msg_.data = steering;
+  
+  //SPEED
+  int speed;
+  double linear;
+
   if(this->config_.invert_speed)
-    this->speed_Int16_msg_.data = -speed;
+    linear = - msg->linear.x;
   else
-    this->speed_Int16_msg_.data = speed;
+    linear = msg->linear.x;
+  
+  min_in=this->config_.linear_sat_min;
+  max_in=this->config_.linear_sat_max;
+  zero_in=0.0;
+
+  linear = this->clip(linear, min_in, max_in);
+
+  zero_out = this->config_.speed_zero;
+
+  if(linear>0)
+  {
+    min_in  = zero_in;
+    min_out = zero_out;
+    max_out = this->config_.speed_max;
+
+  }
+  else //if(linear<=0)
+  {
+    max_in  = zero_in;
+    min_out = this->config_.speed_min;
+    max_out = zero_out;
+  }
+  
+  speed = (((linear  - min_in) * (max_out - min_out)) / (max_in - min_in)) + min_out;
+  
+  this->speed_Int16_msg_.data = speed;
+
+  //PUBLISH
   this->steering_publisher_.publish(this->steering_Int16_msg_);
   this->speed_publisher_.publish(this->speed_Int16_msg_);
+}
 
-  //std::cout << msg->data << std::endl;
-  //unlock previously blocked shared variables
-  //this->alg_.unlock();
-  //this->cmd_vel_mutex_exit();
+double TwistToManualControlAlgNode::clip(double n, double lower, double upper)
+{
+  return std::max(lower, std::min(n, upper));
 }
 
 void TwistToManualControlAlgNode::cmd_vel_mutex_enter(void)