diff --git a/cfg/TwistToManualControl.cfg b/cfg/TwistToManualControl.cfg index c3d51a75a69f454d722f36c00ffab20ef7d12f7b..3fc36d8e0c869df4123dd57f9220d8556a90050b 100755 --- a/cfg/TwistToManualControl.cfg +++ b/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) + diff --git a/launch/node.launch b/launch/node.launch index efe634d32fa178498f7d0c543309709ea77d852d..408e5d9b32e4f613c42dc3b0af39ae34c16b73eb 100644 --- a/launch/node.launch +++ b/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>--> diff --git a/src/twist_to_manual_control_alg_node.cpp b/src/twist_to_manual_control_alg_node.cpp index e4825dcd127bcb9eb7131d34efb29a39f4b80207..df5eeef06897e3ef1bd5d7f47df28832c85f7533 100644 --- a/src/twist_to_manual_control_alg_node.cpp +++ b/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_);