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/config/ps3-noenable.config.yaml b/config/ps3-noenable.config.yaml new file mode 100644 index 0000000..736a0c6 --- /dev/null +++ b/config/ps3-noenable.config.yaml @@ -0,0 +1,9 @@ +axis_linear: 1 +scale_linear: 0.7 +scale_linear_turbo: 1.5 + +axis_angular: 0 +scale_angular: 0.4 + +enable_button: -1 # No need to press the enable_button +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 56c4ca3..7904955 100644 --- a/src/teleop_twist_joy.cpp +++ b/src/teleop_twist_joy.cpp @@ -46,8 +46,8 @@ 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) std::map axis_linear_map; std::map scale_linear_map; @@ -102,26 +102,39 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) 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); 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 +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]) + 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()) { @@ -162,7 +175,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 (joy_msg->buttons[enable_button]) + 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()) { @@ -194,8 +207,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..b9113f4 --- /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: [ 0 ] + 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 } + + +