diff --git a/config/log710.yaml b/config/log710.yaml new file mode 100644 index 00000000..2b87dd39 --- /dev/null +++ b/config/log710.yaml @@ -0,0 +1,59 @@ +# Logitech F710 wireless controller +# Deadman (enable) button: Right Trigger +teleop: + piloting: + type: topic + message_type: "geometry_msgs/Twist" + topic_name: /cmd_vel + deadman_buttons: [7] + axis_mappings: + - + axis: 3 # Right thumb stick (up/down) + target: linear.x + scale: 1.0 + offset: 0.0 + - + axis: 2 # Right thumb stick (left/right) + target: linear.y + scale: -1.0 + offset: 0.0 + - + axis: 1 # Left thumb stick (up/down) + target: linear.z + scale: 1.0 + offset: 0.0 + - + axis: 0 # Left thumb stick (left/right) + target: angular.z + scale: -1.0 + offset: 0.0 + takeoff: + type: topic + message_type: "std_msgs/Empty" + topic_name: takeoff + deadman_buttons: [3, 7] # RT + Y + axis_mappings: [] + land: + type: topic + message_type: "std_msgs/Empty" + topic_name: land + deadman_buttons: [1, 7] # RT + A + axis_mappings: [] + emergency: + type: topic + message_type: "std_msgs/Empty" + topic_name: reset + deadman_buttons: [2, 7] # RT + B + axis_mappings: [] + reset: + type: topic + message_type: "std_msgs/Empty" + topic_name: reset + deadman_buttons: [4, 5] # RT + X + axis_mappings: [] + flip: + type: topic + message_type: "std_msgs/UInt8" + topic_name: flip + deadman_buttons: [6, 7, 0] # RT + LT + X + axis_mappings: [] diff --git a/launch/joy_teleop.launch b/launch/joy_teleop.launch new file mode 100644 index 00000000..ad042f69 --- /dev/null +++ b/launch/joy_teleop.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 8ecb9db0..399fd23e 100644 --- a/package.xml +++ b/package.xml @@ -47,6 +47,8 @@ camera_info_manager message_runtime nav_msgs + joy + joy_teleop diff --git a/src/teleop_twist.cpp b/src/teleop_twist.cpp index dacd2650..303a59e0 100644 --- a/src/teleop_twist.cpp +++ b/src/teleop_twist.cpp @@ -160,10 +160,21 @@ void CmdVelCallback(const geometry_msgs::TwistConstPtr &msg) vp_os_mutex_unlock(&twist_lock); } +void ZeroCmdVel() +{ + cmd_vel.linear.x = 0.0; + cmd_vel.linear.y = 0.0; + cmd_vel.linear.z = 0.0; + cmd_vel.angular.x = 0.0; + cmd_vel.angular.y = 0.0; + cmd_vel.angular.z = 0.0; +} + void LandCallback(const std_msgs::Empty &msg) { vp_os_mutex_lock(&twist_lock); needs_land = true; + ZeroCmdVel(); vp_os_mutex_unlock(&twist_lock); } @@ -171,6 +182,7 @@ void ResetCallback(const std_msgs::Empty &msg) { vp_os_mutex_lock(&twist_lock); needs_reset = true; + ZeroCmdVel(); vp_os_mutex_unlock(&twist_lock); } @@ -178,6 +190,7 @@ void TakeoffCallback(const std_msgs::Empty &msg) { vp_os_mutex_lock(&twist_lock); needs_takeoff = true; + ZeroCmdVel(); vp_os_mutex_unlock(&twist_lock); }