twist_to_manual_control: allow to enable/disable speed and sterring sign or direction

cfg/TwistToManualControl.cfg

@@ -49,6 +49,9 @@ 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)
+
 
 
 

launch/node.launch

@@ -13,8 +13,8 @@
       <remap from="~steering" to="/manual_control/steering"/>
       <remap from="~speed"    to="/manual_control/speed"/>
       
-      <param name="steering_min" value="45"/>
-      <param name="steering_max" value="135"/>
+      <param name="steering_min" value="70"/>
+      <param name="steering_max" value="110"/>
 
       <param name="angular_sat_min" value="-0.5"/>
       <param name="angular_sat_max" value="0.5"/>
@@ -24,6 +24,9 @@
       
       <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"/>
     </node>
 
 <!--  </group>-->

src/twist_to_manual_control_alg_node.cpp

@@ -59,6 +59,8 @@ 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");
 
   //use appropiate mutex to shared variables if necessary
@@ -73,7 +75,17 @@ void TwistToManualControlAlgNode::cmd_vel_callback(const geometry_msgs::Twist::C
   if (angular < this->config_.angular_sat_min)
     angular=this->config_.angular_sat_min;
   
-  int steering = (((angular  - this->config_.angular_sat_min) * (this->config_.steering_max - this->config_.steering_min)) / (this->config_.angular_sat_max - this->config_.angular_sat_min)) + this->config_.steering_min;
+  if(!this->config_.invert_steering)
+  {
+    min=this->config_.steering_min;
+    max=this->config_.steering_max;
+  }
+  else
+  {
+    min=this->config_.steering_max;
+    max=this->config_.steering_min;
+  }
+  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)
@@ -81,12 +93,17 @@ void TwistToManualControlAlgNode::cmd_vel_callback(const geometry_msgs::Twist::C
   
   if (linear < this->config_.linear_sat_min)
     linear=this->config_.linear_sat_min;
-
-  int speed  = (((linear   - this->config_.linear_sat_min)  * (this->config_.speed_max    - this->config_.speed_min))    / (this->config_.linear_sat_max  - this->config_.linear_sat_min))  + this->config_.speed_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;
   
   
   this->steering_Int16_msg_.data = steering;
-  this->speed_Int16_msg_.data = speed;
+  if(this->config_.invert_speed)
+    this->speed_Int16_msg_.data = -speed;
+  else
+    this->speed_Int16_msg_.data = speed;
   this->steering_publisher_.publish(this->steering_Int16_msg_);
   this->speed_publisher_.publish(this->speed_Int16_msg_);