Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Menu based teleop plugins #721

Open
wants to merge 21 commits into
base: master
Choose a base branch
from
Open

Menu based teleop plugins #721

Show file tree
Hide file tree
Changes from 17 commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
ca0849e
Deleted meaningless spaces in joy.py. Fixed typo in joy_pose_6d.py us…
weiqiyang Jun 18, 2019
127eee9
Made (START+SELECT) function as HOME key for Ipega
weiqiyang Jun 18, 2019
9df7e24
Add plugin base_pose, which only allows horizontal movements.
weiqiyang Jun 18, 2019
c2ca9bd
Add sample launch file for joy controlled OverlayMenu and several plu…
weiqiyang Jun 20, 2019
c228a50
Publish base_pos to goal by pressing triangle.
weiqiyang Jul 7, 2019
a565a70
Add pose plugin that publishes a command topic when pressing certain …
weiqiyang Jul 7, 2019
f3437ae
add comments
weiqiyang Jul 7, 2019
4110984
Add joy_bounding_box_selector plugin with a sample launch file.
weiqiyang Jul 7, 2019
17b7920
Fixed publisher behaviour when publish_pose is false. Modified comman…
weiqiyang Jul 9, 2019
f682b90
Fixed a bug caused by empty bounding_box_array. Add command topic.
weiqiyang Jul 9, 2019
e7032e4
Made command text published on button pressing changeable from launch…
weiqiyang Aug 6, 2019
2fe1202
Remove save/load pose function from joy_target_6d (will be applied ba…
weiqiyang Sep 25, 2019
4dac789
Add docstring for the plugins
weiqiyang Oct 23, 2019
5f8c869
Add docstring. Fixed typos
weiqiyang Oct 25, 2019
2f57efa
Fixed typo. Optimized selecting algorithm of bbox.
weiqiyang Oct 28, 2019
da1fdf5
optimized joy_reverse_axis by using toPS3Msg()
weiqiyang Oct 31, 2019
f593931
Fixed bug in base_pose caused by time sync. Optimized queue_size.
weiqiyang Nov 5, 2019
4f8c56a
Renamed sample launch file.
weiqiyang Dec 15, 2019
9304651
fixed typo
weiqiyang Dec 15, 2019
c14340d
fixed typo
weiqiyang Dec 15, 2019
4f7264b
Change to snake case and 4-space indents
weiqiyang Feb 14, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
140 changes: 140 additions & 0 deletions jsk_teleop_joy/launch/config/sample_joy_bounding_box_selector.rviz
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /BoundingBoxArray1
- /BoundingBox1
Splitter Ratio: 0.544444
Tree Height: 565
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: jsk_rviz_plugin/BoundingBoxArray
Enabled: true
Name: BoundingBoxArray
Topic: /bounding_box_array_publisher/output
Unreliable: false
Value: true
alpha: 0.8
color: 25; 255; 0
coloring: Auto
line width: 0.005
only edge: false
show coords: false
- Class: jsk_rviz_plugin/BoundingBox
Enabled: true
Name: BoundingBox
Topic: /selected_bbox
Unreliable: false
Value: true
alpha: 0.8
color: 255; 0; 0
coloring: Flat color
line width: 0.01
only edge: true
show coords: false
- Class: jsk_rviz_plugin/OverlayMenu
Enabled: true
Name: OverlayMenu
Topic: /overlay_menu
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.09505
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.436994
Y: 0.0773152
Z: 0.912857
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.325398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.19358
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 55
Y: 20
44 changes: 44 additions & 0 deletions jsk_teleop_joy/launch/joy_plugin_sample.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
<launch>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

joy_plugin_sample.launch -> sample_joy_plugin.launch

<arg name="CONTROLLER_TYPE" default="auto" />
<arg name="DEV" default="/dev/input/js0" />
<arg name="POSE_REMAP" default="joy_pose" />
<arg name="FRAME_ID" default="base_footprint" />
<node pkg="joy" type="joy_node" name="joy_driver" output="screen" >
<param name="dev" type="string" value="$(arg DEV)" />
<param name="deadzone" value="0.2" />
<param name="autorepeat_rate" value="40" />
<param name="coalesce_interval" value="0.025" />
</node>
<node pkg="jsk_teleop_joy" type="joy.py" name="joy" output="screen"
clear_params="true">
<param name="controller_type" value="$(arg CONTROLLER_TYPE)" />
<rosparam subst_value="true">
plugins:
'JoyTarget6D':
class: 'JoyTarget6D'
args:
publish_pose: True
pose: $(arg POSE_REMAP)
frame_id: $(arg FRAME_ID)
follow_view: true
triangle_cmd: "PREVIEW"
circle_cmd: "EXECUTE"
cross_cmd: "CANCEL"
'Verbose':
class: 'VerboseStatus'
args:
publish_pose: True
pose: $(arg POSE_REMAP)
frame_id: $(arg FRAME_ID)
follow_view: true
'BasePose':
class: 'BasePose'
args:
publish_pose: True
pose: 'base_goal'
frame_id: $(arg FRAME_ID)
follow_view: true
z: 1.0
</rosparam>
</node>
</launch>
72 changes: 72 additions & 0 deletions jsk_teleop_joy/launch/sample_joy_bounding_box_selector.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
<launch>
<arg name="CONTROLLER_TYPE" default="auto" />
<arg name="DEV" default="/dev/input/js0" />
<arg name="POSE_REMAP" default="joy_pose" />
<arg name="FRAME_ID" default="map" />

<arg name="gui" default="true" />

<node name="static_tf_publisher_map_to_shelf"
pkg="tf" type="static_transform_publisher"
args="0 0 1 0 0 0 map shelf_base 100">
</node>

<node name="bounding_box_array_publisher"
pkg="jsk_recognition_utils" type="bounding_box_array_publisher.py">
<rosparam>
frame_id: shelf_base
boxes:
- position: [-0.22, 0.280, 0.361]
dimension: [0.37, 0.248, 0.218]
- position: [-0.22, 0, 0.361]
dimension: [0.37, 0.306, 0.218]
- position: [-0.22, -0.280, 0.361]
dimension: [0.37, 0.248, 0.218]
- position: [-0.22, 0.280, 0.116]
dimension: [0.37, 0.248, 0.192]
- position: [-0.22, 0, 0.116]
dimension: [0.37, 0.306, 0.192]
- position: [-0.22, -0.280, 0.116]
dimension: [0.37, 0.248, 0.192]
- position: [-0.22, 0.280, -0.116]
dimension: [0.37, 0.248, 0.192]
- position: [-0.22, 0, -0.116]
dimension: [0.37, 0.306, 0.192]
- position: [-0.22, -0.280, -0.116]
dimension: [0.37, 0.248, 0.192]
- position: [-0.22, 0.280, -0.361]
dimension: [0.37, 0.248, 0.218]
- position: [-0.22, 0, -0.361]
dimension: [0.37, 0.306, 0.218]
- position: [-0.22, -0.280, -0.361]
dimension: [0.37, 0.248, 0.218]
</rosparam>
</node>

<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find jsk_teleop_joy)/launch/config/sample_joy_bounding_box_selector.rviz">
</node>
</group>

<node pkg="joy" type="joy_node" name="joy_driver" output="screen" >
<param name="dev" type="string" value="$(arg DEV)" />
<param name="deadzone" value="0.2" />
<param name="autorepeat_rate" value="40" />
<param name="coalesce_interval" value="0.025" />
</node>
<node pkg="jsk_teleop_joy" type="joy.py" name="joy" output="screen"
clear_params="true">
<param name="controller_type" value="$(arg CONTROLLER_TYPE)" />
<rosparam subst_value="true">
plugins:
'BBoxSelect':
class: 'JoyBoundingBoxSelector'
args:
frame_id: $(arg FRAME_ID)
input_boxes: '/bounding_box_array_publisher/output'
output: '/selected_bbox'
</rosparam>
</node>
</launch>
8 changes: 8 additions & 0 deletions jsk_teleop_joy/plugin.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,10 @@
</class>
<class name="JoyPose6D" type="jsk_teleop_joy.plugin.joy_pose_6d.JoyPose6D">
</class>
<class name="JoyTarget6D" type="jsk_teleop_joy.plugin.joy_target_6d.JoyTarget6D">
</class>
<class name="BasePose" type="jsk_teleop_joy.plugin.base_pose.BasePose">
</class>
<class name="JoyFootstep" type="jsk_teleop_joy.plugin.joy_footstep.JoyFootstep">
</class>
<class name="JoyFootstepPlanner" type="jsk_teleop_joy.plugin.joy_footstep_planner.JoyFootstepPlanner">
Expand All @@ -30,4 +34,8 @@
</class>
<class name="JoyCmdVel" type="jsk_teleop_joy.plugin.joy_cmd_vel.JoyCmdVel">
</class>
<class name="JoyReverseAxis" type="jsk_teleop_joy.plugin.joy_reverse_axis.JoyReverseAxis">
</class>
<class name="JoyBoundingBoxSelector" type="jsk_teleop_joy.plugin.joy_bounding_box_selector.JoyBoundingBoxSelector">
</class>
</library>
14 changes: 6 additions & 8 deletions jsk_teleop_joy/src/jsk_teleop_joy/joy.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
except:
import roslib; roslib.load_manifest('jsk_teleop_joy')


from sensor_msgs.msg import Joy
from diagnostic_msgs.msg import DiagnosticStatus, DiagnosticArray
import tf.transformations
Expand All @@ -38,12 +37,12 @@ def autoJoyDetect(msg):
elif len(msg.axes) == 20 and len(msg.buttons) == 17:
rospy.loginfo("auto detected as ps3")
AUTO_DETECTED_CLASS = PS3Status
elif len(msg.axes) == 8 and len(msg.buttons) == 19:
elif len(msg.axes) == 8 and (len(msg.buttons) == 16 or len(msg.buttons) == 19):
rospy.loginfo("auto detected as ipega")
AUTO_DETECTED_CLASS = IpegaStatus
else:
AUTO_DETECTED_CLASS = "UNKNOWN"

class JoyManager():
STATE_INITIALIZATION = 1
STATE_RUNNING = 2
Expand All @@ -52,7 +51,7 @@ class JoyManager():
MODE_PLUGIN = 0
MODE_MENU = 1
mode = 0

plugin_instances = []
def stateDiagnostic(self, stat):
if self.state == self.STATE_INITIALIZATION:
Expand Down Expand Up @@ -99,7 +98,7 @@ def __init__(self, plugin_package="jsk_teleop_joy"):
elif self.controller_type == 'ipega':
self.JoyStatus = IpegaStatus
elif self.controller_type == 'auto':
s = rospy.Subscriber('/joy', Joy, autoJoyDetect)
s = rospy.Subscriber('/joy', Joy, autoJoyDetect, queue_size=1)
self.state = self.STATE_WAIT_FOR_JOY
error_message_published = False
r = rospy.Rate(1)
Expand Down Expand Up @@ -190,7 +189,7 @@ def joyCB(self, msg):
self.pre_status = status
self.history.add(status)
self.diagnostic_updater.update()

def main():
global g_manager
rospy.sleep(1)
Expand All @@ -202,7 +201,6 @@ def main():
return False
else:
rospy.spin()

if __name__ == '__main__':
main()

9 changes: 7 additions & 2 deletions jsk_teleop_joy/src/jsk_teleop_joy/joy_status.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,14 +117,13 @@ class IpegaStatus(JoyStatus):
def __init__(self, msg):
'''
ipaga game pad
No home button
Y button => triangle
X button => square
B button => circle
A button => cross
START + SELECT => HOME(center)
'''
JoyStatus.__init__(self)
self.center = False
if msg.buttons[10] == 1:
self.select = True
else:
Expand Down Expand Up @@ -189,6 +188,12 @@ def __init__(self, msg):
self.R2 = True
else:
self.R2 = False
if self.start and self.select:
self.start = False
self.select = False
self.center = True
else:
self.center = False
self.left_analog_x = msg.axes[0]
self.left_analog_y = msg.axes[1]
self.right_analog_x = msg.axes[2]
Expand Down
Loading