diff --git a/cfg/TwistToManualControl.cfg b/cfg/TwistToManualControl.cfg
index 3fc36d8e0c869df4123dd57f9220d8556a90050b..50e2aba3087aec816c519f61dfd65d5da26fceec 100755
--- a/cfg/TwistToManualControl.cfg
+++ b/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)
diff --git a/config/ps3.yaml b/config/ps3.yaml
index 993c3dc7898f3681379ff603ed0a370a48120a29..c66e7f5515fb22fda0f6aeb609eb8974324a1af1 100644
--- a/config/ps3.yaml
+++ b/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
diff --git a/include/twist_to_manual_control_alg_node.h b/include/twist_to_manual_control_alg_node.h
index fa73d60b5d17424d4e4f2f358fc848b1aa753a7f..81dcd9730132ed21058ec92a355b6b33b3cea77c 100644
--- a/include/twist_to_manual_control_alg_node.h
+++ b/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
diff --git a/launch/node.launch b/launch/node.launch
index 408e5d9b32e4f613c42dc3b0af39ae34c16b73eb..4c779e603e3ab7597febf22326f3627ca461855f 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -1,7 +1,9 @@
<!-- -->
<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>-->
diff --git a/src/twist_to_manual_control_alg_node.cpp b/src/twist_to_manual_control_alg_node.cpp
index df5eeef06897e3ef1bd5d7f47df28832c85f7533..e3250cb33b8cdc9ded9c5b88cede6b4f002d5643 100644
--- a/src/twist_to_manual_control_alg_node.cpp
+++ b/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)