From e6a27af5c52f769e7c389643372a4b5fa3b203a5 Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Thu, 12 May 2016 14:24:29 +0200 Subject: [PATCH 01/10] enable_button can be disabled by assinging to it value -1 .Because without enable_button the no-movement command would never be sent (it gets sent when enable_button is released) a breaking button was added which does exactly that. break_button is by default disabled (assigned to button -1). If enabled, breaking takes preference over any other kind of movement. --- src/teleop_twist_joy.cpp | 54 ++++++++++++++++++++++++++++------------ 1 file changed, 38 insertions(+), 16 deletions(-) diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index 56c4ca3..ab36fc9 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -46,8 +46,9 @@ struct TeleopTwistJoy::Impl ros::Subscriber joy_sub; ros::Publisher cmd_vel_pub; - int enable_button; - int enable_turbo_button; + int enable_button; // Enable normal motion. Defaults to joystick button 0 + int enable_turbo_button; // Enable sprint by using alternative gain. By default disabled (-1) + int break_button; // Send break no-motion command. By default disabled (-1) std::map axis_linear_map; std::map scale_linear_map; @@ -74,6 +75,7 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) nh_param->param("enable_button", pimpl_->enable_button, 0); nh_param->param("enable_turbo_button", pimpl_->enable_turbo_button, -1); + nh_param->param("break_button", pimpl_->break_button, -1); if (nh_param->getParam("axis_linear", pimpl_->axis_linear_map)) { @@ -101,27 +103,44 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) nh_param->param("scale_angular_turbo", pimpl_->scale_angular_turbo_map["yaw"], pimpl_->scale_angular_map["yaw"]); } - - ROS_INFO_NAMED("TeleopTwistJoy", "Teleop enable button %i.", pimpl_->enable_button); - ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", - "Turbo on button %i.", pimpl_->enable_turbo_button); + + ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, + "TeleopTwistJoy", + "Teleop enable button %i.", + pimpl_->enable_button); + ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, + "TeleopTwistJoy", + "Turbo on button %i.", + pimpl_->enable_turbo_button); + ROS_INFO_COND_NAMED(pimpl_->break_button >= 0, + "TeleopTwistJoy", + "Breaking on button %i.", + pimpl_->break_button); for (std::map::iterator it = pimpl_->axis_linear_map.begin(); it != pimpl_->axis_linear_map.end(); ++it) { - ROS_INFO_NAMED("TeleopTwistJoy", "Linear axis %s on %i at scale %f.", - it->first.c_str(), it->second, pimpl_->scale_linear_map[it->first]); - ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", - "Turbo for linear axis %s is scale %f.", it->first.c_str(), pimpl_->scale_linear_turbo_map[it->first]); + ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, + "TeleopTwistJoy", + "Linear axis %s on %i at scale %f.", + it->first.c_str(), it->second, pimpl_->scale_linear_map[it->first]); + ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, + "TeleopTwistJoy", + "Turbo for linear axis %s is scale %f.", + it->first.c_str(), pimpl_->scale_linear_turbo_map[it->first]); } for (std::map::iterator it = pimpl_->axis_angular_map.begin(); it != pimpl_->axis_angular_map.end(); ++it) { - ROS_INFO_NAMED("TeleopTwistJoy", "Angular axis %s on %i at scale %f.", - it->first.c_str(), it->second, pimpl_->scale_angular_map[it->first]); - ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", - "Turbo for angular axis %s is scale %f.", it->first.c_str(), pimpl_->scale_angular_turbo_map[it->first]); + ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, + "TeleopTwistJoy", + "Angular axis %s on %i at scale %f.", + it->first.c_str(), it->second, pimpl_->scale_angular_map[it->first]); + ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, + "TeleopTwistJoy", + "Turbo for angular axis %s is scale %f.", + it->first.c_str(), pimpl_->scale_angular_turbo_map[it->first]); } pimpl_->sent_disable_msg = false; @@ -132,7 +151,9 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg // Initializes with zeros by default. geometry_msgs::Twist cmd_vel_msg; - if (enable_turbo_button >= 0 && joy_msg->buttons[enable_turbo_button]) + if (enable_turbo_button >= 0 && // turbo_button enabled AND + joy_msg->buttons[enable_turbo_button] && // turbo_button pressed AND + !joy_msg->buttons[break_button] ) // break_button not pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) { @@ -162,7 +183,8 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg cmd_vel_pub.publish(cmd_vel_msg); sent_disable_msg = false; } - else if (joy_msg->buttons[enable_button]) + else if ( (enable_button < 0 || joy_msg->buttons[enable_button]) && // (enable_button not enabled OR enable_button pressed ) AND + !joy_msg->buttons[break_button] ) // breaking_button not pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) { From 51e94e8e4881eb4d2c76844ead6a35c8d6e09ff1 Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Thu, 12 May 2016 14:25:13 +0200 Subject: [PATCH 02/10] Added breaking button with always enabled movement config-file example --- config/ps3-breaking.config.yaml | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 config/ps3-breaking.config.yaml diff --git a/config/ps3-breaking.config.yaml b/config/ps3-breaking.config.yaml new file mode 100644 index 0000000..8e62e8c --- /dev/null +++ b/config/ps3-breaking.config.yaml @@ -0,0 +1,10 @@ +axis_linear: 1 +scale_linear: 0.7 +scale_linear_turbo: 1.5 + +axis_angular: 0 +scale_angular: 0.4 + +enable_button: -1 # Movement always enabled +enable_turbo_button: 10 # L1 shoulder button +break_button: 8 # L2 shoulder button \ No newline at end of file From 3f471932192d413fc5b80593f55b4bc038d4b2f7 Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Thu, 12 May 2016 15:35:50 +0200 Subject: [PATCH 03/10] Fixed typo. Renamed break_button to brake_button as suggested by Mike Purvis --- src/teleop_twist_joy.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index ab36fc9..9d18ca2 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -48,7 +48,7 @@ struct TeleopTwistJoy::Impl int enable_button; // Enable normal motion. Defaults to joystick button 0 int enable_turbo_button; // Enable sprint by using alternative gain. By default disabled (-1) - int break_button; // Send break no-motion command. By default disabled (-1) + int brake_button; // Send break no-motion command. By default disabled (-1) std::map axis_linear_map; std::map scale_linear_map; @@ -75,7 +75,7 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) nh_param->param("enable_button", pimpl_->enable_button, 0); nh_param->param("enable_turbo_button", pimpl_->enable_turbo_button, -1); - nh_param->param("break_button", pimpl_->break_button, -1); + nh_param->param("brake_button", pimpl_->brake_button, -1); if (nh_param->getParam("axis_linear", pimpl_->axis_linear_map)) { @@ -112,10 +112,10 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) "TeleopTwistJoy", "Turbo on button %i.", pimpl_->enable_turbo_button); - ROS_INFO_COND_NAMED(pimpl_->break_button >= 0, + ROS_INFO_COND_NAMED(pimpl_->brake_button >= 0, "TeleopTwistJoy", "Breaking on button %i.", - pimpl_->break_button); + pimpl_->brake_button); for (std::map::iterator it = pimpl_->axis_linear_map.begin(); it != pimpl_->axis_linear_map.end(); ++it) @@ -153,7 +153,7 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg if (enable_turbo_button >= 0 && // turbo_button enabled AND joy_msg->buttons[enable_turbo_button] && // turbo_button pressed AND - !joy_msg->buttons[break_button] ) // break_button not pressed + !joy_msg->buttons[brake_button] ) // brake_button not pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) { @@ -184,7 +184,7 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg sent_disable_msg = false; } else if ( (enable_button < 0 || joy_msg->buttons[enable_button]) && // (enable_button not enabled OR enable_button pressed ) AND - !joy_msg->buttons[break_button] ) // breaking_button not pressed + !joy_msg->buttons[brake_button] ) // breaking_button not pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) { From 80dd095a175541074996788416f5afcecfe8b87d Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Thu, 12 May 2016 17:34:49 +0200 Subject: [PATCH 04/10] Fixed typo --- config/{ps3-breaking.config.yaml => ps3-braking.config.yaml} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename config/{ps3-breaking.config.yaml => ps3-braking.config.yaml} (56%) diff --git a/config/ps3-breaking.config.yaml b/config/ps3-braking.config.yaml similarity index 56% rename from config/ps3-breaking.config.yaml rename to config/ps3-braking.config.yaml index 8e62e8c..5ce8b5a 100644 --- a/config/ps3-breaking.config.yaml +++ b/config/ps3-braking.config.yaml @@ -5,6 +5,6 @@ scale_linear_turbo: 1.5 axis_angular: 0 scale_angular: 0.4 -enable_button: -1 # Movement always enabled +enable_button: -1 # No need to press the enable_button enable_turbo_button: 10 # L1 shoulder button -break_button: 8 # L2 shoulder button \ No newline at end of file +brake_button: 8 # L2 shoulder button \ No newline at end of file From 48edca15203d145eef247bc3ab0aaf59a04f8c1e Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Thu, 12 May 2016 17:37:45 +0200 Subject: [PATCH 05/10] Fixed typo --- src/teleop_twist_joy.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index 9d18ca2..0e4f25f 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -48,7 +48,7 @@ struct TeleopTwistJoy::Impl int enable_button; // Enable normal motion. Defaults to joystick button 0 int enable_turbo_button; // Enable sprint by using alternative gain. By default disabled (-1) - int brake_button; // Send break no-motion command. By default disabled (-1) + int brake_button; // Send brake no-motion command. By default disabled (-1) std::map axis_linear_map; std::map scale_linear_map; From 426bfb0f8cfbe761ee4c6e5b98e0783e7b58d5ee Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Thu, 12 May 2016 18:23:40 +0200 Subject: [PATCH 06/10] Fixed linter errors: trailing whitespaces, use of tabs and lines longer than 120 chars --- src/teleop_twist_joy.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index 0e4f25f..ac6235b 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -46,9 +46,9 @@ struct TeleopTwistJoy::Impl ros::Subscriber joy_sub; ros::Publisher cmd_vel_pub; - int enable_button; // Enable normal motion. Defaults to joystick button 0 - int enable_turbo_button; // Enable sprint by using alternative gain. By default disabled (-1) - int brake_button; // Send brake no-motion command. By default disabled (-1) + int enable_button; // Enable normal motion. Defaults to joystick button 0 + int enable_turbo_button; // Enable sprint by using alternative gain. By default disabled (-1) + int brake_button; // Send brake no-motion command. By default disabled (-1) std::map axis_linear_map; std::map scale_linear_map; @@ -103,18 +103,18 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) nh_param->param("scale_angular_turbo", pimpl_->scale_angular_turbo_map["yaw"], pimpl_->scale_angular_map["yaw"]); } - - ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, + + ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, "TeleopTwistJoy", - "Teleop enable button %i.", + "Teleop enable button %i.", pimpl_->enable_button); - ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, + ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", - "Turbo on button %i.", + "Turbo on button %i.", pimpl_->enable_turbo_button); - ROS_INFO_COND_NAMED(pimpl_->brake_button >= 0, + ROS_INFO_COND_NAMED(pimpl_->brake_button >= 0, "TeleopTwistJoy", - "Breaking on button %i.", + "Breaking on button %i.", pimpl_->brake_button); for (std::map::iterator it = pimpl_->axis_linear_map.begin(); @@ -122,24 +122,24 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) { ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, "TeleopTwistJoy", - "Linear axis %s on %i at scale %f.", + "Linear axis %s on %i at scale %f.", it->first.c_str(), it->second, pimpl_->scale_linear_map[it->first]); ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", - "Turbo for linear axis %s is scale %f.", + "Turbo for linear axis %s is scale %f.", it->first.c_str(), pimpl_->scale_linear_turbo_map[it->first]); } for (std::map::iterator it = pimpl_->axis_angular_map.begin(); it != pimpl_->axis_angular_map.end(); ++it) { - ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, + ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, "TeleopTwistJoy", "Angular axis %s on %i at scale %f.", it->first.c_str(), it->second, pimpl_->scale_angular_map[it->first]); - ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, + ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", - "Turbo for angular axis %s is scale %f.", + "Turbo for angular axis %s is scale %f.", it->first.c_str(), pimpl_->scale_angular_turbo_map[it->first]); } @@ -183,7 +183,7 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg cmd_vel_pub.publish(cmd_vel_msg); sent_disable_msg = false; } - else if ( (enable_button < 0 || joy_msg->buttons[enable_button]) && // (enable_button not enabled OR enable_button pressed ) AND + else if ( (enable_button < 0 || joy_msg->buttons[enable_button]) && // (movement enabled) AND !joy_msg->buttons[brake_button] ) // breaking_button not pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) From fa195b2bdb3c41814670c2a617ad5a87c1fba88c Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Fri, 13 May 2016 10:19:50 +0200 Subject: [PATCH 07/10] Removed unwanted extra spaces pointed out by [tonybaltovski@github] --- src/teleop_twist_joy.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index ac6235b..9ce658c 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -120,14 +120,14 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) for (std::map::iterator it = pimpl_->axis_linear_map.begin(); it != pimpl_->axis_linear_map.end(); ++it) { - ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, - "TeleopTwistJoy", - "Linear axis %s on %i at scale %f.", - it->first.c_str(), it->second, pimpl_->scale_linear_map[it->first]); - ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, - "TeleopTwistJoy", - "Turbo for linear axis %s is scale %f.", - it->first.c_str(), pimpl_->scale_linear_turbo_map[it->first]); + ROS_INFO_COND_NAMED(pimpl_->enable_button >= 0, + "TeleopTwistJoy", + "Linear axis %s on %i at scale %f.", + it->first.c_str(), it->second, pimpl_->scale_linear_map[it->first]); + ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, + "TeleopTwistJoy", + "Turbo for linear axis %s is scale %f.", + it->first.c_str(), pimpl_->scale_linear_turbo_map[it->first]); } for (std::map::iterator it = pimpl_->axis_angular_map.begin(); From 157754ca344985a858b4c2d271a416ae307504c0 Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Fri, 10 Jun 2016 10:34:08 +0200 Subject: [PATCH 08/10] Testing enable button optional --- ...g.config.yaml => ps3-noenable.config.yaml} | 3 +-- src/teleop_twist_joy.cpp | 18 ++++---------- test/enable_button_disabled_joy.test | 22 +++++++++++++++++ ...able_button_disabled_turbo_enable_joy.test | 24 +++++++++++++++++++ 4 files changed, 52 insertions(+), 15 deletions(-) rename config/{ps3-braking.config.yaml => ps3-noenable.config.yaml} (63%) create mode 100644 test/enable_button_disabled_joy.test create mode 100644 test/enable_button_disabled_turbo_enable_joy.test diff --git a/config/ps3-braking.config.yaml b/config/ps3-noenable.config.yaml similarity index 63% rename from config/ps3-braking.config.yaml rename to config/ps3-noenable.config.yaml index 5ce8b5a..736a0c6 100644 --- a/config/ps3-braking.config.yaml +++ b/config/ps3-noenable.config.yaml @@ -6,5 +6,4 @@ axis_angular: 0 scale_angular: 0.4 enable_button: -1 # No need to press the enable_button -enable_turbo_button: 10 # L1 shoulder button -brake_button: 8 # L2 shoulder button \ No newline at end of file +enable_turbo_button: 10 # L1 shoulder button \ No newline at end of file diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index 9ce658c..cfdbddc 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -26,6 +26,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "ros/ros.h" #include "sensor_msgs/Joy.h" #include "teleop_twist_joy/teleop_twist_joy.h" +#include #include #include @@ -48,7 +49,6 @@ struct TeleopTwistJoy::Impl int enable_button; // Enable normal motion. Defaults to joystick button 0 int enable_turbo_button; // Enable sprint by using alternative gain. By default disabled (-1) - int brake_button; // Send brake no-motion command. By default disabled (-1) std::map axis_linear_map; std::map scale_linear_map; @@ -75,7 +75,6 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) nh_param->param("enable_button", pimpl_->enable_button, 0); nh_param->param("enable_turbo_button", pimpl_->enable_turbo_button, -1); - nh_param->param("brake_button", pimpl_->brake_button, -1); if (nh_param->getParam("axis_linear", pimpl_->axis_linear_map)) { @@ -112,10 +111,6 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) "TeleopTwistJoy", "Turbo on button %i.", pimpl_->enable_turbo_button); - ROS_INFO_COND_NAMED(pimpl_->brake_button >= 0, - "TeleopTwistJoy", - "Breaking on button %i.", - pimpl_->brake_button); for (std::map::iterator it = pimpl_->axis_linear_map.begin(); it != pimpl_->axis_linear_map.end(); ++it) @@ -151,9 +146,7 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg // Initializes with zeros by default. geometry_msgs::Twist cmd_vel_msg; - if (enable_turbo_button >= 0 && // turbo_button enabled AND - joy_msg->buttons[enable_turbo_button] && // turbo_button pressed AND - !joy_msg->buttons[brake_button] ) // brake_button not pressed + if (enable_turbo_button >= 0 && joy_msg->buttons[enable_turbo_button]) // Turbo button enabled AND pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) { @@ -183,8 +176,7 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg cmd_vel_pub.publish(cmd_vel_msg); sent_disable_msg = false; } - else if ( (enable_button < 0 || joy_msg->buttons[enable_button]) && // (movement enabled) AND - !joy_msg->buttons[brake_button] ) // breaking_button not pressed + else if (joy_msg->buttons[enable_button] || enable_button < 0) // enable_button pressed OR enable_button disabled { if (axis_linear_map.find("x") != axis_linear_map.end()) { @@ -216,8 +208,8 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg } else { - // When enable button is released, immediately send a single no-motion command - // in order to stop the robot. + // When enable_button is released, immediately send a single no-motion command + // in order to stop the robot, unless enable_button is disabled (-1) if (!sent_disable_msg) { cmd_vel_pub.publish(cmd_vel_msg); diff --git a/test/enable_button_disabled_joy.test b/test/enable_button_disabled_joy.test new file mode 100644 index 0000000..53d9fb9 --- /dev/null +++ b/test/enable_button_disabled_joy.test @@ -0,0 +1,22 @@ + + + + axis_linear: 1 + axis_angular: 0 + scale_linear: 2.0 + scale_angular: 3.0 + enable_button: -1 + + + + + + publish_joy: + axes: [ 0.3, 0.4 ] + buttons: [ ] + expect_cmd_vel: + linear: { x: 0.8, y: 0, z: 0 } + angular: { x: 0, y: 0, z: 0.9 } + + + diff --git a/test/enable_button_disabled_turbo_enable_joy.test b/test/enable_button_disabled_turbo_enable_joy.test new file mode 100644 index 0000000..7e1ce5b --- /dev/null +++ b/test/enable_button_disabled_turbo_enable_joy.test @@ -0,0 +1,24 @@ + + + + axis_linear: 1 + axis_angular: 0 + scale_linear: 1.0 + scale_linear_turbo: 2.0 + scale_angular: 3.0 + enable_button: -1 + enable_turbo_button: 1 + + + + + + publish_joy: + axes: [ 0.3, 0.4 ] + buttons: [ 0, 1 ] + expect_cmd_vel: + linear: { x: 0.8, y: 0, z: 0 } + angular: { x: 0, y: 0, z: 0.9 } + + + From db2f8cb5c2feb0c47373d27b0be995ab3f95fb9e Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Fri, 10 Jun 2016 10:58:14 +0200 Subject: [PATCH 09/10] Fixed lint errors and added travis test for disabled enable_button --- CMakeLists.txt | 2 ++ src/teleop_twist_joy.cpp | 3 +-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 865abcb..03950f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,4 +53,6 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/only_turbo_joy.test) add_rostest(test/turbo_angular_enable_joy.test) add_rostest(test/turbo_angular_enable_joy_with_rosparam_map.test) + add_rostest(test/enable_button_disabled_joy.test) + add_rostest(test/enable_button_disabled_turbo_enable_joy.test) endif() diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index cfdbddc..7904955 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -26,7 +26,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "ros/ros.h" #include "sensor_msgs/Joy.h" #include "teleop_twist_joy/teleop_twist_joy.h" -#include #include #include @@ -146,7 +145,7 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg // Initializes with zeros by default. geometry_msgs::Twist cmd_vel_msg; - if (enable_turbo_button >= 0 && joy_msg->buttons[enable_turbo_button]) // Turbo button enabled AND pressed + if (enable_turbo_button >= 0 && joy_msg->buttons[enable_turbo_button]) // Turbo button enabled AND pressed { if (axis_linear_map.find("x") != axis_linear_map.end()) { From 2796d301d34d80b048bb900cf2408af8b12deb0f Mon Sep 17 00:00:00 2001 From: Andres Gongora Date: Fri, 10 Jun 2016 11:10:33 +0200 Subject: [PATCH 10/10] Changed button array not to be empty. Added a 0 --- test/enable_button_disabled_joy.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/enable_button_disabled_joy.test b/test/enable_button_disabled_joy.test index 53d9fb9..b9113f4 100644 --- a/test/enable_button_disabled_joy.test +++ b/test/enable_button_disabled_joy.test @@ -13,7 +13,7 @@ publish_joy: axes: [ 0.3, 0.4 ] - buttons: [ ] + buttons: [ 0 ] expect_cmd_vel: linear: { x: 0.8, y: 0, z: 0 } angular: { x: 0, y: 0, z: 0.9 }