refactored the code, fixed config bug
This commit is contained in:
Pavel Lutskov
@@ -6,7 +6,7 @@
|
||||
"lt": 0.5666157603263855,
|
||||
"lr": 0.6060229593671598,
|
||||
"cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864],
|
||||
"arm": 0.66392470071039278
|
||||
"arm": 0.66392470071039278,
|
||||
"should": {
|
||||
"L": [0, 0.08, 0.15],
|
||||
"R": [0, -0.08, 0.15]
|
||||
|
||||
12
launch/calibration.launch
Normal file
12
launch/calibration.launch
Normal file
@@ -0,0 +1,12 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<!--Launch file for Test of teleoperating NAO project-->
|
||||
|
||||
<launch>
|
||||
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
|
||||
<node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
|
||||
output="screen"/>
|
||||
<node name="speech_server" pkg="teleoperation" type="speech_server.py"
|
||||
output="screen"/>
|
||||
<node name="calibrator" pkg="teleoperation" type="calibrator.py"/>
|
||||
</launch>
|
||||
@@ -15,4 +15,14 @@
|
||||
output="screen"/>
|
||||
<node name="walker" pkg="teleoperation" type="walker.py"/>
|
||||
<node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
|
||||
<node name="human" pkg="teleoperation" type="rviz_human"/>
|
||||
|
||||
<!-- open rviz window showing nao -->
|
||||
<node name="rviz_nao" pkg="rviz" type="rviz" args =
|
||||
" -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
|
||||
|
||||
<!-- open rviz window showing human -->
|
||||
<node name="rviz_human" pkg="rviz" type="rviz"
|
||||
args=" -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
|
||||
|
||||
</launch>
|
||||
|
||||
@@ -7,11 +7,10 @@ Panels:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Grid1
|
||||
- /Grid1/Offset1
|
||||
- /TF1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 359
|
||||
Tree Height: 775
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
@@ -47,340 +46,48 @@ Visualization Manager:
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Z: -1
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: odom
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
Aruco_0_frame:
|
||||
Value: true
|
||||
Aruco_1_frame:
|
||||
Value: true
|
||||
Aruco_2_frame:
|
||||
Value: true
|
||||
CameraBottom_frame:
|
||||
Value: false
|
||||
CameraBottom_optical_frame:
|
||||
Value: false
|
||||
CameraTop_frame:
|
||||
Value: false
|
||||
CameraTop_optical_frame:
|
||||
Value: false
|
||||
ChestButton_frame:
|
||||
Value: false
|
||||
Head:
|
||||
Value: false
|
||||
HeadTouchFront_frame:
|
||||
Value: false
|
||||
HeadTouchMiddle_frame:
|
||||
Value: false
|
||||
HeadTouchRear_frame:
|
||||
Value: false
|
||||
ImuTorsoAccelerometer_frame:
|
||||
Value: false
|
||||
ImuTorsoGyrometer_frame:
|
||||
Value: false
|
||||
LAnklePitch:
|
||||
Value: false
|
||||
LBicep:
|
||||
Value: false
|
||||
LElbow:
|
||||
Value: false
|
||||
LFinger11_link:
|
||||
Value: false
|
||||
LFinger12_link:
|
||||
Value: false
|
||||
LFinger13_link:
|
||||
Value: false
|
||||
LFinger21_link:
|
||||
Value: false
|
||||
LFinger22_link:
|
||||
Value: false
|
||||
LFinger23_link:
|
||||
Value: false
|
||||
LFootBumperLeft_frame:
|
||||
Value: false
|
||||
LFootBumperRight_frame:
|
||||
Value: false
|
||||
LForeArm:
|
||||
Value: false
|
||||
LFsrFL_frame:
|
||||
Value: false
|
||||
LFsrFR_frame:
|
||||
Value: false
|
||||
LFsrRL_frame:
|
||||
Value: false
|
||||
LFsrRR_frame:
|
||||
Value: false
|
||||
LHandTouchBack_frame:
|
||||
Value: false
|
||||
LHandTouchLeft_frame:
|
||||
Value: false
|
||||
LHandTouchRight_frame:
|
||||
Value: false
|
||||
LHip:
|
||||
Value: false
|
||||
LInfraRed_frame:
|
||||
Value: false
|
||||
LPelvis:
|
||||
Value: false
|
||||
LShoulder:
|
||||
Value: false
|
||||
LSonar_frame:
|
||||
Value: false
|
||||
LThigh:
|
||||
Value: false
|
||||
LThumb1_link:
|
||||
Value: false
|
||||
LThumb2_link:
|
||||
Value: false
|
||||
LTibia:
|
||||
Value: false
|
||||
MicroFrontCenter_frame:
|
||||
Value: false
|
||||
MicroRearCenter_frame:
|
||||
Value: false
|
||||
MicroSurroundLeft_frame:
|
||||
Value: false
|
||||
MicroSurroundRight_frame:
|
||||
Value: false
|
||||
Neck:
|
||||
Value: false
|
||||
RAnklePitch:
|
||||
Value: false
|
||||
RBicep:
|
||||
Value: false
|
||||
RElbow:
|
||||
Value: false
|
||||
RFinger11_link:
|
||||
Value: false
|
||||
RFinger12_link:
|
||||
Value: false
|
||||
RFinger13_link:
|
||||
Value: false
|
||||
RFinger21_link:
|
||||
Value: false
|
||||
RFinger22_link:
|
||||
Value: false
|
||||
RFinger23_link:
|
||||
Value: false
|
||||
RFootBumperLeft_frame:
|
||||
Value: false
|
||||
RFootBumperRight_frame:
|
||||
Value: false
|
||||
RForeArm:
|
||||
Value: false
|
||||
RFsrFL_frame:
|
||||
Value: false
|
||||
RFsrFR_frame:
|
||||
Value: false
|
||||
RFsrRL_frame:
|
||||
Value: false
|
||||
RFsrRR_frame:
|
||||
Value: false
|
||||
RHandTouchBack_frame:
|
||||
Value: false
|
||||
RHandTouchLeft_frame:
|
||||
Value: false
|
||||
RHandTouchRight_frame:
|
||||
Value: false
|
||||
RHip:
|
||||
Value: false
|
||||
RInfraRed_frame:
|
||||
Value: false
|
||||
RPelvis:
|
||||
Value: false
|
||||
RShoulder:
|
||||
Value: false
|
||||
RSonar_frame:
|
||||
Value: false
|
||||
RThigh:
|
||||
Value: false
|
||||
RThumb1_link:
|
||||
Value: false
|
||||
RThumb2_link:
|
||||
Value: false
|
||||
RTibia:
|
||||
Value: false
|
||||
base_footprint:
|
||||
Value: false
|
||||
base_link:
|
||||
Value: false
|
||||
gaze:
|
||||
Value: false
|
||||
l_ankle:
|
||||
Value: false
|
||||
l_gripper:
|
||||
Value: false
|
||||
l_sole:
|
||||
Value: false
|
||||
l_wrist:
|
||||
Value: false
|
||||
odom:
|
||||
Value: false
|
||||
r_ankle:
|
||||
Value: false
|
||||
r_gripper:
|
||||
Value: false
|
||||
r_sole:
|
||||
Value: false
|
||||
r_wrist:
|
||||
Value: false
|
||||
torso:
|
||||
Value: false
|
||||
Marker Scale: 0.1
|
||||
Marker Scale: 0.2
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: false
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
odom:
|
||||
base_link:
|
||||
base_footprint:
|
||||
{}
|
||||
torso:
|
||||
ChestButton_frame:
|
||||
{}
|
||||
ImuTorsoAccelerometer_frame:
|
||||
{}
|
||||
ImuTorsoGyrometer_frame:
|
||||
{}
|
||||
LPelvis:
|
||||
LHip:
|
||||
LThigh:
|
||||
LTibia:
|
||||
LAnklePitch:
|
||||
l_ankle:
|
||||
LFootBumperLeft_frame:
|
||||
{}
|
||||
LFootBumperRight_frame:
|
||||
{}
|
||||
LFsrFL_frame:
|
||||
{}
|
||||
LFsrFR_frame:
|
||||
{}
|
||||
LFsrRL_frame:
|
||||
{}
|
||||
LFsrRR_frame:
|
||||
{}
|
||||
l_sole:
|
||||
{}
|
||||
LShoulder:
|
||||
LBicep:
|
||||
LElbow:
|
||||
LForeArm:
|
||||
l_wrist:
|
||||
LFinger11_link:
|
||||
LFinger12_link:
|
||||
LFinger13_link:
|
||||
{}
|
||||
LFinger21_link:
|
||||
LFinger22_link:
|
||||
LFinger23_link:
|
||||
{}
|
||||
LHandTouchBack_frame:
|
||||
{}
|
||||
LHandTouchLeft_frame:
|
||||
{}
|
||||
LHandTouchRight_frame:
|
||||
{}
|
||||
LThumb1_link:
|
||||
LThumb2_link:
|
||||
{}
|
||||
l_gripper:
|
||||
{}
|
||||
LSonar_frame:
|
||||
{}
|
||||
Neck:
|
||||
Head:
|
||||
CameraBottom_frame:
|
||||
CameraBottom_optical_frame:
|
||||
{}
|
||||
CameraTop_frame:
|
||||
CameraTop_optical_frame:
|
||||
Aruco_0_frame:
|
||||
{}
|
||||
Aruco_1_frame:
|
||||
{}
|
||||
Aruco_2_frame:
|
||||
{}
|
||||
HeadTouchFront_frame:
|
||||
{}
|
||||
HeadTouchMiddle_frame:
|
||||
{}
|
||||
HeadTouchRear_frame:
|
||||
{}
|
||||
LInfraRed_frame:
|
||||
{}
|
||||
MicroFrontCenter_frame:
|
||||
{}
|
||||
MicroRearCenter_frame:
|
||||
{}
|
||||
MicroSurroundLeft_frame:
|
||||
{}
|
||||
MicroSurroundRight_frame:
|
||||
{}
|
||||
RInfraRed_frame:
|
||||
{}
|
||||
gaze:
|
||||
{}
|
||||
RPelvis:
|
||||
RHip:
|
||||
RThigh:
|
||||
RTibia:
|
||||
RAnklePitch:
|
||||
r_ankle:
|
||||
RFootBumperLeft_frame:
|
||||
{}
|
||||
RFootBumperRight_frame:
|
||||
{}
|
||||
RFsrFL_frame:
|
||||
{}
|
||||
RFsrFR_frame:
|
||||
{}
|
||||
RFsrRL_frame:
|
||||
{}
|
||||
RFsrRR_frame:
|
||||
{}
|
||||
r_sole:
|
||||
{}
|
||||
RShoulder:
|
||||
RBicep:
|
||||
RElbow:
|
||||
RForeArm:
|
||||
r_wrist:
|
||||
RFinger11_link:
|
||||
RFinger12_link:
|
||||
RFinger13_link:
|
||||
{}
|
||||
RFinger21_link:
|
||||
RFinger22_link:
|
||||
RFinger23_link:
|
||||
{}
|
||||
RHandTouchBack_frame:
|
||||
{}
|
||||
RHandTouchLeft_frame:
|
||||
{}
|
||||
RHandTouchRight_frame:
|
||||
{}
|
||||
RThumb1_link:
|
||||
RThumb2_link:
|
||||
{}
|
||||
r_gripper:
|
||||
{}
|
||||
RSonar_frame:
|
||||
{}
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: false
|
||||
- Class: rviz/Marker
|
||||
Enabled: true
|
||||
Marker Topic: visualization_marker
|
||||
Name: Marker
|
||||
Namespaces:
|
||||
base: true
|
||||
body: true
|
||||
camera: true
|
||||
head: true
|
||||
l_arm: true
|
||||
l_joints: true
|
||||
l_leg: true
|
||||
l_legjoints: true
|
||||
l_shoulder: true
|
||||
r_arm: true
|
||||
r_leg: true
|
||||
r_shoulder: true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: torso
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
@@ -401,30 +108,30 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 1.25714
|
||||
Distance: 8.05248
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
X: -0.098179
|
||||
Y: 0.0497635
|
||||
Z: 0.136802
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 0.280398
|
||||
Pitch: 0.3654
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 6.11041
|
||||
Yaw: 3.14176
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 663
|
||||
Height: 1056
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000279fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000000000000279000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002f80000027900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
@@ -433,6 +140,6 @@ Window Geometry:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 760
|
||||
X: 1015
|
||||
Y: 14
|
||||
Width: 1855
|
||||
X: 65
|
||||
Y: 274
|
||||
|
||||
@@ -7,10 +7,10 @@ Panels:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Grid1
|
||||
- /Grid1/Offset1
|
||||
- /RobotModel1
|
||||
- /TF1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 286
|
||||
Tree Height: 895
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
@@ -51,320 +51,415 @@ Visualization Manager:
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: odom
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
CameraBottom_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
CameraBottom_optical_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
CameraTop_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
CameraTop_optical_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
ChestButton_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Head:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
HeadTouchFront_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
HeadTouchMiddle_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
HeadTouchRear_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
ImuTorsoAccelerometer_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
ImuTorsoGyrometer_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LAnklePitch:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LBicep:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LElbow:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LFinger11_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFinger12_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFinger13_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFinger21_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFinger22_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFinger23_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFootBumperLeft_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LFootBumperRight_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LForeArm:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFsrFL_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFsrFR_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFsrRL_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LFsrRR_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LHandTouchBack_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LHandTouchLeft_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LHandTouchRight_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LHip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LInfraRed_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LPelvis:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LShoulder:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LSonar_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
LThigh:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LThumb1_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LThumb2_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
LTibia:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
MicroFrontCenter_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
MicroRearCenter_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
MicroSurroundLeft_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
MicroSurroundRight_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Neck:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RAnklePitch:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RBicep:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RElbow:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RFinger11_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFinger12_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFinger13_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFinger21_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFinger22_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFinger23_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFootBumperLeft_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RFootBumperRight_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RForeArm:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFsrFL_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFsrFR_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFsrRL_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RFsrRR_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RHandTouchBack_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RHandTouchLeft_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RHandTouchRight_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RHip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RInfraRed_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RPelvis:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RShoulder:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RSonar_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
RThigh:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RThumb1_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RThumb2_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RTibia:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
gaze:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
l_ankle:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_gripper:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
l_sole:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
l_wrist:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_ankle:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_gripper:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
r_sole:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
r_wrist:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
torso:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/TF
|
||||
Enabled: false
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
CameraBottom_frame:
|
||||
Value: true
|
||||
CameraBottom_optical_frame:
|
||||
Value: true
|
||||
CameraTop_frame:
|
||||
Value: true
|
||||
CameraTop_optical_frame:
|
||||
Value: true
|
||||
ChestButton_frame:
|
||||
Value: true
|
||||
Head:
|
||||
Value: true
|
||||
HeadTouchFront_frame:
|
||||
Value: true
|
||||
HeadTouchMiddle_frame:
|
||||
Value: true
|
||||
HeadTouchRear_frame:
|
||||
Value: true
|
||||
ImuTorsoAccelerometer_frame:
|
||||
Value: true
|
||||
ImuTorsoGyrometer_frame:
|
||||
Value: true
|
||||
LAnklePitch:
|
||||
Value: true
|
||||
LBicep:
|
||||
Value: true
|
||||
LElbow:
|
||||
Value: true
|
||||
LFinger11_link:
|
||||
Value: true
|
||||
LFinger12_link:
|
||||
Value: true
|
||||
LFinger13_link:
|
||||
Value: true
|
||||
LFinger21_link:
|
||||
Value: true
|
||||
LFinger22_link:
|
||||
Value: true
|
||||
LFinger23_link:
|
||||
Value: true
|
||||
LFootBumperLeft_frame:
|
||||
Value: true
|
||||
LFootBumperRight_frame:
|
||||
Value: true
|
||||
LForeArm:
|
||||
Value: true
|
||||
LFsrFL_frame:
|
||||
Value: true
|
||||
LFsrFR_frame:
|
||||
Value: true
|
||||
LFsrRL_frame:
|
||||
Value: true
|
||||
LFsrRR_frame:
|
||||
Value: true
|
||||
LHandTouchBack_frame:
|
||||
Value: true
|
||||
LHandTouchLeft_frame:
|
||||
Value: true
|
||||
LHandTouchRight_frame:
|
||||
Value: true
|
||||
LHip:
|
||||
Value: true
|
||||
LInfraRed_frame:
|
||||
Value: true
|
||||
LPelvis:
|
||||
Value: true
|
||||
LShoulder:
|
||||
Value: true
|
||||
LSonar_frame:
|
||||
Value: true
|
||||
LThigh:
|
||||
Value: true
|
||||
LThumb1_link:
|
||||
Value: true
|
||||
LThumb2_link:
|
||||
Value: true
|
||||
LTibia:
|
||||
Value: true
|
||||
MicroFrontCenter_frame:
|
||||
Value: true
|
||||
MicroRearCenter_frame:
|
||||
Value: true
|
||||
MicroSurroundLeft_frame:
|
||||
Value: true
|
||||
MicroSurroundRight_frame:
|
||||
Value: true
|
||||
Neck:
|
||||
Value: true
|
||||
RAnklePitch:
|
||||
Value: true
|
||||
RBicep:
|
||||
Value: true
|
||||
RElbow:
|
||||
Value: true
|
||||
RFinger11_link:
|
||||
Value: true
|
||||
RFinger12_link:
|
||||
Value: true
|
||||
RFinger13_link:
|
||||
Value: true
|
||||
RFinger21_link:
|
||||
Value: true
|
||||
RFinger22_link:
|
||||
Value: true
|
||||
RFinger23_link:
|
||||
Value: true
|
||||
RFootBumperLeft_frame:
|
||||
Value: true
|
||||
RFootBumperRight_frame:
|
||||
Value: true
|
||||
RForeArm:
|
||||
Value: true
|
||||
RFsrFL_frame:
|
||||
Value: true
|
||||
RFsrFR_frame:
|
||||
Value: true
|
||||
RFsrRL_frame:
|
||||
Value: true
|
||||
RFsrRR_frame:
|
||||
Value: true
|
||||
RHandTouchBack_frame:
|
||||
Value: true
|
||||
RHandTouchLeft_frame:
|
||||
Value: true
|
||||
RHandTouchRight_frame:
|
||||
Value: true
|
||||
RHip:
|
||||
Value: true
|
||||
RInfraRed_frame:
|
||||
Value: true
|
||||
RPelvis:
|
||||
Value: true
|
||||
RShoulder:
|
||||
Value: true
|
||||
RSonar_frame:
|
||||
Value: true
|
||||
RThigh:
|
||||
Value: true
|
||||
RThumb1_link:
|
||||
Value: true
|
||||
RThumb2_link:
|
||||
Value: true
|
||||
RTibia:
|
||||
Value: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
gaze:
|
||||
Value: true
|
||||
l_ankle:
|
||||
Value: true
|
||||
l_gripper:
|
||||
Value: true
|
||||
l_sole:
|
||||
Value: true
|
||||
l_wrist:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
r_ankle:
|
||||
Value: true
|
||||
r_gripper:
|
||||
Value: true
|
||||
r_sole:
|
||||
Value: true
|
||||
r_wrist:
|
||||
Value: true
|
||||
torso:
|
||||
Value: true
|
||||
Marker Scale: 0.1
|
||||
Marker Scale: 0.3
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: false
|
||||
Show Names: false
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
odom:
|
||||
base_link:
|
||||
base_footprint:
|
||||
{}
|
||||
torso:
|
||||
ChestButton_frame:
|
||||
{}
|
||||
ImuTorsoAccelerometer_frame:
|
||||
{}
|
||||
ImuTorsoGyrometer_frame:
|
||||
{}
|
||||
LPelvis:
|
||||
LHip:
|
||||
LThigh:
|
||||
LTibia:
|
||||
LAnklePitch:
|
||||
l_ankle:
|
||||
LFootBumperLeft_frame:
|
||||
{}
|
||||
LFootBumperRight_frame:
|
||||
{}
|
||||
LFsrFL_frame:
|
||||
{}
|
||||
LFsrFR_frame:
|
||||
{}
|
||||
LFsrRL_frame:
|
||||
{}
|
||||
LFsrRR_frame:
|
||||
{}
|
||||
l_sole:
|
||||
{}
|
||||
LShoulder:
|
||||
LBicep:
|
||||
LElbow:
|
||||
LForeArm:
|
||||
l_wrist:
|
||||
LFinger11_link:
|
||||
LFinger12_link:
|
||||
LFinger13_link:
|
||||
{}
|
||||
LFinger21_link:
|
||||
LFinger22_link:
|
||||
LFinger23_link:
|
||||
{}
|
||||
LHandTouchBack_frame:
|
||||
{}
|
||||
LHandTouchLeft_frame:
|
||||
{}
|
||||
LHandTouchRight_frame:
|
||||
{}
|
||||
LThumb1_link:
|
||||
LThumb2_link:
|
||||
{}
|
||||
l_gripper:
|
||||
{}
|
||||
LSonar_frame:
|
||||
{}
|
||||
Neck:
|
||||
Head:
|
||||
CameraBottom_frame:
|
||||
CameraBottom_optical_frame:
|
||||
{}
|
||||
CameraTop_frame:
|
||||
CameraTop_optical_frame:
|
||||
{}
|
||||
HeadTouchFront_frame:
|
||||
{}
|
||||
HeadTouchMiddle_frame:
|
||||
{}
|
||||
HeadTouchRear_frame:
|
||||
{}
|
||||
LInfraRed_frame:
|
||||
{}
|
||||
MicroFrontCenter_frame:
|
||||
{}
|
||||
MicroRearCenter_frame:
|
||||
{}
|
||||
MicroSurroundLeft_frame:
|
||||
{}
|
||||
MicroSurroundRight_frame:
|
||||
{}
|
||||
RInfraRed_frame:
|
||||
{}
|
||||
gaze:
|
||||
{}
|
||||
RPelvis:
|
||||
RHip:
|
||||
RThigh:
|
||||
RTibia:
|
||||
RAnklePitch:
|
||||
r_ankle:
|
||||
RFootBumperLeft_frame:
|
||||
{}
|
||||
RFootBumperRight_frame:
|
||||
{}
|
||||
RFsrFL_frame:
|
||||
{}
|
||||
RFsrFR_frame:
|
||||
{}
|
||||
RFsrRL_frame:
|
||||
{}
|
||||
RFsrRR_frame:
|
||||
{}
|
||||
r_sole:
|
||||
{}
|
||||
RShoulder:
|
||||
RBicep:
|
||||
RElbow:
|
||||
RForeArm:
|
||||
r_wrist:
|
||||
RFinger11_link:
|
||||
RFinger12_link:
|
||||
RFinger13_link:
|
||||
{}
|
||||
RFinger21_link:
|
||||
RFinger22_link:
|
||||
RFinger23_link:
|
||||
{}
|
||||
RHandTouchBack_frame:
|
||||
{}
|
||||
RHandTouchLeft_frame:
|
||||
{}
|
||||
RHandTouchRight_frame:
|
||||
{}
|
||||
RThumb1_link:
|
||||
RThumb2_link:
|
||||
{}
|
||||
r_gripper:
|
||||
{}
|
||||
RSonar_frame:
|
||||
{}
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Value: false
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
@@ -389,7 +484,7 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 1.11097
|
||||
Distance: 1.26786
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
@@ -401,18 +496,18 @@ Visualization Manager:
|
||||
Z: 0
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 0.300398
|
||||
Pitch: 0.574797
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.1404
|
||||
Yaw: 3.1104
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 459
|
||||
Height: 1176
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000001adfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000000000001ad000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000019b000001ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000040efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000040e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000040e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
@@ -421,6 +516,6 @@ Window Geometry:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 411
|
||||
X: 55
|
||||
Y: 14
|
||||
Width: 1855
|
||||
X: 1985
|
||||
Y: 24
|
||||
|
||||
@@ -5,12 +5,12 @@ import json
|
||||
from math import asin, atan, radians
|
||||
|
||||
import numpy as np
|
||||
from naoqi import ALProxy
|
||||
|
||||
from masterloop import mp
|
||||
|
||||
|
||||
FRAME_TORSO = 0
|
||||
K = 0.1
|
||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
||||
|
||||
_here = os.path.dirname(os.path.realpath(__file__))
|
||||
with open('{}/../config/default.yaml'.format(_here)) as f:
|
||||
@@ -63,7 +63,7 @@ def our_cartesian(my_xyz, side):
|
||||
nao_xyz = to_nao(my_xyz, side)
|
||||
delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
|
||||
crt_q = mp.getAngles([side + j for j in JOINTS], False)
|
||||
delta_q = np.linalg.pinv(jacobian()).dot(delta_r).flatten()
|
||||
delta_q = np.linalg.pinv(jacobian(side)).dot(delta_r).flatten()
|
||||
return crt_q + delta_q
|
||||
|
||||
|
||||
|
||||
@@ -4,7 +4,7 @@ import os
|
||||
import rospy
|
||||
from naoqi import ALProxy, ALBroker, ALModule
|
||||
|
||||
from masterloop import inform_masterloop_factory
|
||||
from masterloop import inform_masterloop_factory, mp
|
||||
|
||||
|
||||
fall_broker = None
|
||||
@@ -18,7 +18,6 @@ class FallDetectorModule(ALModule):
|
||||
|
||||
def __init__(self, name):
|
||||
ALModule.__init__(self, name)
|
||||
self.mp = ALProxy('ALMotion')
|
||||
|
||||
def on_robot_falling(self, *_args):
|
||||
_inform_masterloop('falling')
|
||||
@@ -26,9 +25,11 @@ class FallDetectorModule(ALModule):
|
||||
def on_robot_fallen(self, *_args):
|
||||
if not _inform_masterloop('fallen'):
|
||||
return
|
||||
self.mp.rest()
|
||||
mp.rest()
|
||||
rospy.Rate(0.5).sleep()
|
||||
self.mp.wakeUp()
|
||||
mp.wakeUp()
|
||||
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
||||
am.setExpressiveListeningEnabled(False)
|
||||
pp = ALProxy('ALRobotPosture')
|
||||
while not pp.goToPosture('StandInit', 1.0):
|
||||
pass
|
||||
|
||||
@@ -1,12 +1,10 @@
|
||||
#! /usr/bin/env python
|
||||
import os
|
||||
|
||||
import rospy
|
||||
import tf
|
||||
from naoqi import ALProxy
|
||||
|
||||
from masterloop import inform_masterloop_factory
|
||||
from controller import movement, dumb, mp
|
||||
from controller import movement, dumb, our_cartesian
|
||||
|
||||
|
||||
_inform_masterloop = inform_masterloop_factory('imitator')
|
||||
@@ -21,27 +19,22 @@ if __name__ == '__main__':
|
||||
|
||||
ll = tf.TransformListener()
|
||||
|
||||
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
||||
am.setExpressiveListeningEnabled(False)
|
||||
|
||||
mp.wakeUp()
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
rospy.Rate(10).sleep()
|
||||
if not _inform_masterloop('imitate'):
|
||||
continue
|
||||
rospy.logdebug('IMITATOR: ACTIVE')
|
||||
|
||||
if TORSO:
|
||||
try:
|
||||
_, q = ll.lookupTransform('odom',
|
||||
'Aruco_0_frame',
|
||||
rospy.Time(0))
|
||||
rot = tf.transformations.euler_from_quaternion(q)
|
||||
mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
|
||||
[-rot[1], -rot[1]], 0.3)
|
||||
except Exception as e:
|
||||
rospy.logwarn(e)
|
||||
# if TORSO:
|
||||
# try:
|
||||
# _, q = ll.lookupTransform('odom',
|
||||
# 'Aruco_0_frame',
|
||||
# rospy.Time(0))
|
||||
# rot = tf.transformations.euler_from_quaternion(q)
|
||||
# mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
|
||||
# [-rot[1], -rot[1]], 0.3)
|
||||
# except Exception as e:
|
||||
# rospy.logwarn(e)
|
||||
|
||||
for i, side in enumerate(['L', 'R'], 1):
|
||||
try:
|
||||
@@ -54,6 +47,4 @@ if __name__ == '__main__':
|
||||
rospy.logwarn(e)
|
||||
continue
|
||||
|
||||
movement(my_arm_xyz, side, dumb)
|
||||
|
||||
mp.rest()
|
||||
movement(my_arm_xyz, side, our_cartesian)
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#! /usr/bin/env python
|
||||
import os
|
||||
from argparse import ArgumentParser
|
||||
|
||||
import rospy
|
||||
@@ -7,6 +8,7 @@ import actionlib
|
||||
from teleoperation.srv import InformMasterloop, InformMasterloopResponse
|
||||
from teleoperation.srv import Hands
|
||||
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
|
||||
from naoqi import ALProxy
|
||||
|
||||
|
||||
# Constants
|
||||
@@ -32,6 +34,8 @@ state = 'dead' # Also walk, imitate, fallen, idle
|
||||
hand_state = 'closed'
|
||||
speech_in_progress = False
|
||||
|
||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
||||
|
||||
|
||||
def init_voc_state_speech():
|
||||
global VOC_STATE, SPEECH_TRANSITIONS
|
||||
@@ -144,15 +148,24 @@ if __name__ == '__main__':
|
||||
AI = args.autoimitate
|
||||
init_voc_state_speech()
|
||||
|
||||
ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
|
||||
|
||||
speech = actionlib.SimpleActionClient('speech_server',
|
||||
RequestSpeechAction)
|
||||
|
||||
speech.wait_for_server()
|
||||
rospy.loginfo('SPEECH: SERVER THERE')
|
||||
|
||||
rospy.on_shutdown(lambda: speech_in_progress and speech.cancel_goal())
|
||||
mp.wakeUp()
|
||||
mp.moveInit()
|
||||
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
||||
am.setExpressiveListeningEnabled(False)
|
||||
|
||||
ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
|
||||
|
||||
def _shutdown():
|
||||
if speech_in_progress:
|
||||
speech.cancel_goal()
|
||||
mp.rest()
|
||||
|
||||
rospy.on_shutdown(_shutdown)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
if state in ('idle', 'imitate', 'dead'):
|
||||
|
||||
@@ -5,9 +5,8 @@ from time import sleep
|
||||
|
||||
import rospy
|
||||
import tf
|
||||
from naoqi import ALProxy
|
||||
|
||||
from masterloop import inform_masterloop_factory
|
||||
from masterloop import inform_masterloop_factory, mp
|
||||
|
||||
|
||||
FW = None
|
||||
@@ -60,9 +59,6 @@ if __name__ == '__main__':
|
||||
global_init()
|
||||
rospy.wait_for_service('inform_masterloop')
|
||||
ll = tf.TransformListener()
|
||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
||||
mp.wakeUp()
|
||||
mp.moveInit()
|
||||
mp.setMoveArmsEnabled(False, False)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
|
||||
@@ -4,401 +4,534 @@
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
struct quarternion {
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
double w;
|
||||
};
|
||||
|
||||
|
||||
quarternion toQuaternion(double yaw, double pitch, double roll);
|
||||
|
||||
|
||||
int main( int argc, char** argv )
|
||||
{
|
||||
ros::init(argc, argv, "rviz_human");
|
||||
ros::NodeHandle n;
|
||||
ros::Rate r(100);
|
||||
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
|
||||
"visualization_marker", 10
|
||||
);
|
||||
ros::init(argc, argv, "rviz_human");
|
||||
ros::NodeHandle n;
|
||||
ros::Rate r(100);
|
||||
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
|
||||
"visualization_marker", 15);
|
||||
|
||||
tf::TransformListener tll;
|
||||
tf::TransformListener aruco_0;
|
||||
tf::TransformListener aruco_1;
|
||||
tf::TransformListener aruco_2;
|
||||
|
||||
int i = 0;
|
||||
int walk = 0;
|
||||
float l_leg_y = 0.0;
|
||||
float r_leg_y = 0.0;
|
||||
// suppose we already have the position of the center marker in the correct frame
|
||||
|
||||
// suppose we already have the position of the center marker
|
||||
// in the correct frame
|
||||
geometry_msgs::Point torso;
|
||||
geometry_msgs::Point l_hand;
|
||||
geometry_msgs::Point r_hand;
|
||||
geometry_msgs::Point base_position;
|
||||
|
||||
float x_start = 2;
|
||||
float y_start = 0;
|
||||
float z_start = 1.3;
|
||||
// Set human to starting postion
|
||||
torso.x = -1;
|
||||
torso.y = 0;
|
||||
torso.z = 0;
|
||||
|
||||
float x_0 = 2;
|
||||
float y_0 = 0;
|
||||
float z_0 = 1.3;
|
||||
l_hand.x = -1;
|
||||
l_hand.y = 0.35;
|
||||
l_hand.z = -0.2;
|
||||
|
||||
float x_1 = 0;
|
||||
float y_1 = 0;
|
||||
float z_1 = 0;
|
||||
r_hand.x = -1;
|
||||
r_hand.y = -0.35;
|
||||
r_hand.z = -0.2;
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
base_position.x = -1;
|
||||
base_position.y = 0;
|
||||
base_position.z = -1;
|
||||
|
||||
// tried to subscribe to tf to recieve marker coordinates
|
||||
tf::StampedTransform aruco_0_tf;
|
||||
tf::StampedTransform aruco_1_tf;
|
||||
tf::StampedTransform aruco_2_tf;
|
||||
double base_r = 2;
|
||||
|
||||
try {
|
||||
tll.lookupTransform(
|
||||
"/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
|
||||
/*ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
|
||||
ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
|
||||
ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
|
||||
*/
|
||||
|
||||
tll.lookupTransform(
|
||||
"/Aruco_0_frame", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
|
||||
/*
|
||||
ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
|
||||
ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
|
||||
ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
|
||||
*/
|
||||
|
||||
x_0 = aruco_0_tf.getOrigin()[0];
|
||||
y_0 = aruco_0_tf.getOrigin()[1];
|
||||
z_0 = aruco_0_tf.getOrigin()[2];
|
||||
|
||||
x_1 = aruco_1_tf.getOrigin()[0];
|
||||
y_1 = aruco_1_tf.getOrigin()[1];
|
||||
z_1 = aruco_1_tf.getOrigin()[2];
|
||||
}
|
||||
|
||||
/*
|
||||
catch (tf::TransformException ex){
|
||||
ROS_ERROR("%s",ex.what());
|
||||
ros::Duration(1.0).sleep();
|
||||
}
|
||||
|
||||
try{
|
||||
*/
|
||||
|
||||
catch (tf::TransformException ex) {
|
||||
ROS_ERROR("%s",ex.what());
|
||||
ros::Duration(1.0).sleep();
|
||||
}
|
||||
|
||||
|
||||
visualization_msgs::Marker body;
|
||||
visualization_msgs::Marker head;
|
||||
visualization_msgs::Marker l_leg;
|
||||
visualization_msgs::Marker r_leg;
|
||||
visualization_msgs::Marker l_arm;
|
||||
visualization_msgs::Marker r_arm;
|
||||
|
||||
visualization_msgs::Marker camera;
|
||||
|
||||
// Set the frame ID and timestamp.
|
||||
// See the TF tutorials for information on these.
|
||||
body.header.frame_id = "/odom";
|
||||
body.header.stamp = ros::Time::now();
|
||||
|
||||
head.header.frame_id = "/odom";
|
||||
head.header.stamp = ros::Time::now();
|
||||
|
||||
l_leg.header.frame_id = "/odom";
|
||||
l_leg.header.stamp = ros::Time::now();
|
||||
|
||||
r_leg.header.frame_id = "/odom";
|
||||
r_leg.header.stamp = ros::Time::now();
|
||||
|
||||
l_arm.header.frame_id = "/odom";
|
||||
l_arm.header.stamp = ros::Time::now();
|
||||
|
||||
r_arm.header.frame_id = "/odom";
|
||||
r_arm.header.stamp = ros::Time::now();
|
||||
|
||||
camera.header.frame_id = "/odom";
|
||||
camera.header.stamp = ros::Time::now();
|
||||
|
||||
body.ns = "body";
|
||||
body.id = 0;
|
||||
|
||||
head.ns = "head";
|
||||
head.id = 1;
|
||||
|
||||
l_leg.ns = "l_leg";
|
||||
l_leg.id = 2;
|
||||
|
||||
r_leg.ns = "r_leg";
|
||||
r_leg.id = 3;
|
||||
|
||||
l_arm.ns = "l_arm";
|
||||
l_arm.id = 4;
|
||||
|
||||
r_arm.ns = "r_arm";
|
||||
r_arm.id = 5;
|
||||
|
||||
camera.ns = "r_arm";
|
||||
camera.id = 6;
|
||||
|
||||
body.type = visualization_msgs::Marker::CUBE;
|
||||
head.type = visualization_msgs::Marker::SPHERE;
|
||||
l_leg.type = visualization_msgs::Marker::CYLINDER;
|
||||
r_leg.type = visualization_msgs::Marker::CYLINDER;
|
||||
l_arm.type = visualization_msgs::Marker::CYLINDER;
|
||||
r_arm.type = visualization_msgs::Marker::CYLINDER;
|
||||
camera.type = visualization_msgs::Marker::CUBE;
|
||||
|
||||
// Set the marker action.
|
||||
// Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
|
||||
|
||||
body.action = visualization_msgs::Marker::ADD;
|
||||
head.action = visualization_msgs::Marker::ADD;
|
||||
l_arm.action = visualization_msgs::Marker::ADD;
|
||||
r_arm.action = visualization_msgs::Marker::ADD;
|
||||
l_leg.action = visualization_msgs::Marker::ADD;
|
||||
r_arm.action = visualization_msgs::Marker::ADD;
|
||||
camera.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
|
||||
// Set the pose of the marker.
|
||||
// This is a full 6DOF pose relative to the
|
||||
// frame/time specified in the header
|
||||
|
||||
body.pose.position.x = x_0;
|
||||
body.pose.position.y = y_0;
|
||||
body.pose.position.z = z_0;
|
||||
body.pose.orientation.x = 0.0;
|
||||
body.pose.orientation.y = 0.0;
|
||||
body.pose.orientation.z = 0.0;
|
||||
body.pose.orientation.w = 1.0;
|
||||
|
||||
head.pose.position.x = x_0;
|
||||
head.pose.position.y = y_0;
|
||||
head.pose.position.z = z_0+0.5; //1.85
|
||||
head.pose.orientation.x = 0.0;
|
||||
head.pose.orientation.y = 0.0;
|
||||
head.pose.orientation.z = 0.0;
|
||||
head.pose.orientation.w = 1.0;
|
||||
|
||||
l_leg.pose.position.x = x_0;
|
||||
l_leg.pose.position.y = y_0+0.2;
|
||||
l_leg.pose.position.z = z_0-0.8; //0.4
|
||||
l_leg.pose.orientation.x = 0.0;
|
||||
l_leg.pose.orientation.y = l_leg_y;
|
||||
l_leg.pose.orientation.z = 0.0;
|
||||
l_leg.pose.orientation.w = 1.0;
|
||||
|
||||
r_leg.pose.position.x = x_0;
|
||||
r_leg.pose.position.y = y_0-0.2;
|
||||
r_leg.pose.position.z = z_0-0.8; //0.4
|
||||
r_leg.pose.orientation.x = 0.0;
|
||||
r_leg.pose.orientation.y = r_leg_y;
|
||||
r_leg.pose.orientation.z = 0.0;
|
||||
r_leg.pose.orientation.w = 1.0;
|
||||
|
||||
// calculate left arm angel in z y plane; arm length is 0.5
|
||||
|
||||
float alpha = atan(z_1/y_1);
|
||||
float delta_z = 0;//0.25*sin(alpha);
|
||||
float delta_y = 0;//0.35*cos(alpha);
|
||||
|
||||
//rad to degree
|
||||
alpha = alpha*180/3.1415;
|
||||
quarternion q = toQuaternion(0,0, alpha);
|
||||
|
||||
ROS_INFO("x: %f, y: %f, z: %f", x_1, y_1, z_1);
|
||||
|
||||
ROS_INFO("alpha: %f, z: %f, y: %f", alpha, delta_z, delta_y);
|
||||
|
||||
ROS_INFO("qx: %f, qy: %f, qz: %f, qw: %f", q.x,q.y,q.z,q.w);
|
||||
|
||||
|
||||
l_arm.pose.position.x = x_0;
|
||||
l_arm.pose.position.y = y_0+0.4+delta_y;
|
||||
l_arm.pose.position.z = z_0+0.3+delta_z; //1.6
|
||||
l_arm.pose.orientation.x = 0.0+q.x;//+alpha;
|
||||
l_arm.pose.orientation.y = 0.0+q.y;
|
||||
l_arm.pose.orientation.z = 0.0+q.z;
|
||||
l_arm.pose.orientation.w = 1.0+q.w;
|
||||
|
||||
r_arm.pose.position.x = x_0;
|
||||
r_arm.pose.position.y = y_0-0.4;
|
||||
r_arm.pose.position.z = z_0+0.3; //1.6
|
||||
r_arm.pose.orientation.x = 1.0;
|
||||
r_arm.pose.orientation.y = 0.0;
|
||||
r_arm.pose.orientation.z = 0.0;
|
||||
r_arm.pose.orientation.w = 1.0;
|
||||
|
||||
camera.pose.position.x = 0;
|
||||
camera.pose.position.y = 0;
|
||||
camera.pose.position.z = 0.5;
|
||||
camera.pose.orientation.x = 0.0;
|
||||
camera.pose.orientation.y = 0.0;
|
||||
camera.pose.orientation.z = 0.0;
|
||||
camera.pose.orientation.w = 0.0;
|
||||
|
||||
|
||||
// Set the scale of the marker -- 1x1x1 here means 1m on a side
|
||||
|
||||
body.scale.x = 0.2;
|
||||
body.scale.y = 0.4;
|
||||
body.scale.z = 0.8;
|
||||
|
||||
head.scale.x = 0.3;
|
||||
head.scale.y = 0.3;
|
||||
head.scale.z = 0.4;
|
||||
|
||||
l_leg.scale.x = 0.2;
|
||||
l_leg.scale.y = 0.2;
|
||||
l_leg.scale.z = 0.9;
|
||||
|
||||
r_leg.scale.x = 0.2;
|
||||
r_leg.scale.y = 0.2;
|
||||
r_leg.scale.z = 0.9;
|
||||
|
||||
l_arm.scale.x = 0.2;
|
||||
l_arm.scale.y = 0.2;
|
||||
l_arm.scale.z = 0.5;
|
||||
|
||||
r_arm.scale.x = 0.2;
|
||||
r_arm.scale.y = 0.2;
|
||||
r_arm.scale.z = 0.5;
|
||||
|
||||
camera.scale.x = 0.5;
|
||||
camera.scale.y = 0.5;
|
||||
camera.scale.z = 1;
|
||||
|
||||
// Set the color -- be sure to set alpha to something non-zero!
|
||||
|
||||
body.color.r = 0.0f;
|
||||
body.color.g = 1.0f;
|
||||
body.color.b = 0.5f;
|
||||
body.color.a = 1.0;
|
||||
|
||||
head.color.r = 0.0f;
|
||||
head.color.g = 1.0f;
|
||||
head.color.b = 0.0f;
|
||||
head.color.a = 1.0;
|
||||
|
||||
l_leg.color.r = 1.0f;
|
||||
l_leg.color.g = 0.5f;
|
||||
l_leg.color.b = 0.0f;
|
||||
l_leg.color.a = 1.0;
|
||||
|
||||
r_leg.color.r = 1.0f;
|
||||
r_leg.color.g = 0.5f;
|
||||
r_leg.color.b = 0.0f;
|
||||
r_leg.color.a = 1.0;
|
||||
|
||||
l_arm.color.r = 1.0f;
|
||||
l_arm.color.g = 0.5f;
|
||||
l_arm.color.b = 1.0f;
|
||||
l_arm.color.a = 1.0;
|
||||
|
||||
r_arm.color.r = 1.0f;
|
||||
r_arm.color.g = 0.5f;
|
||||
r_arm.color.b = 1.0f;
|
||||
r_arm.color.a = 1.0;
|
||||
|
||||
camera.color.r = 1.0f;
|
||||
camera.color.g = 1.0f;
|
||||
camera.color.b = 1.0f;
|
||||
camera.color.a = 1.0;
|
||||
|
||||
|
||||
body.lifetime = ros::Duration();
|
||||
head.lifetime = ros::Duration();
|
||||
l_leg.lifetime = ros::Duration();
|
||||
r_leg.lifetime = ros::Duration();
|
||||
l_arm.lifetime = ros::Duration();
|
||||
r_arm.lifetime = ros::Duration();
|
||||
camera.lifetime = ros::Duration();
|
||||
|
||||
|
||||
// Publish the marker
|
||||
|
||||
while (marker_pub.getNumSubscribers() < 1)
|
||||
while (ros::ok())
|
||||
{
|
||||
if (!ros::ok())
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
ROS_WARN_ONCE("Please create a subscriber to the marker");
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
// tried to subscribe to tf to recieve marker coordinates
|
||||
tf::StampedTransform aruco_0_tf;
|
||||
tf::StampedTransform aruco_1_tf;
|
||||
tf::StampedTransform aruco_2_tf;
|
||||
|
||||
try{
|
||||
aruco_0.lookupTransform("/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
|
||||
ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
|
||||
ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
|
||||
ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
|
||||
|
||||
aruco_1.lookupTransform("/odom", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
|
||||
ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
|
||||
ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
|
||||
ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
|
||||
|
||||
aruco_2.lookupTransform("/odom", "/Aruco_2_frame",ros::Time(0), aruco_2_tf);
|
||||
ROS_INFO("0_2: %f",aruco_2_tf.getOrigin()[0]);
|
||||
ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]);
|
||||
ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]);
|
||||
|
||||
// update torso and hand positions
|
||||
torso.x = aruco_0_tf.getOrigin()[0];
|
||||
torso.y = aruco_0_tf.getOrigin()[1];
|
||||
torso.z = aruco_0_tf.getOrigin()[2];
|
||||
|
||||
l_hand.x = aruco_1_tf.getOrigin()[0];
|
||||
l_hand.y = aruco_1_tf.getOrigin()[1];
|
||||
l_hand.z = aruco_1_tf.getOrigin()[2];
|
||||
|
||||
r_hand.x = aruco_2_tf.getOrigin()[0];
|
||||
r_hand.y = aruco_2_tf.getOrigin()[1];
|
||||
r_hand.z = aruco_2_tf.getOrigin()[2];
|
||||
|
||||
// little walking animation
|
||||
i++;
|
||||
if(i % 100 == 0)
|
||||
{
|
||||
if(walk == 0)
|
||||
{
|
||||
walk = 1;
|
||||
l_leg_y = 0.2;
|
||||
r_leg_y = -0.2;
|
||||
}
|
||||
else
|
||||
{
|
||||
l_leg_y = -0.2;
|
||||
r_leg_y = 0.2;
|
||||
walk = 0;
|
||||
catch (tf::TransformException ex){
|
||||
ROS_INFO("%s",ex.what());
|
||||
ros::Duration(1.0).sleep();
|
||||
}
|
||||
|
||||
visualization_msgs::Marker body;
|
||||
visualization_msgs::Marker head;
|
||||
visualization_msgs::Marker l_shoulder;
|
||||
visualization_msgs::Marker r_shoulder;
|
||||
visualization_msgs::Marker l_joints;
|
||||
visualization_msgs::Marker r_joints;
|
||||
visualization_msgs::Marker l_arm;
|
||||
visualization_msgs::Marker r_arm;
|
||||
visualization_msgs::Marker l_legjoints;
|
||||
visualization_msgs::Marker r_legjoints;
|
||||
visualization_msgs::Marker l_leg;
|
||||
visualization_msgs::Marker r_leg;
|
||||
visualization_msgs::Marker camera;
|
||||
visualization_msgs::Marker base;
|
||||
|
||||
// Set the frame ID and timestamp
|
||||
|
||||
body.header.frame_id = "/odom";
|
||||
body.header.stamp = ros::Time::now();
|
||||
|
||||
head.header.frame_id = "/odom";
|
||||
head.header.stamp = ros::Time::now();
|
||||
|
||||
l_shoulder.header.frame_id = "/odom";
|
||||
l_shoulder.header.stamp = ros::Time::now();
|
||||
|
||||
r_shoulder.header.frame_id = "/odom";
|
||||
r_shoulder.header.stamp = ros::Time::now();
|
||||
|
||||
l_joints.header.frame_id = "/odom";
|
||||
l_joints.header.stamp = ros::Time::now();
|
||||
|
||||
r_joints.header.frame_id = "/odom";
|
||||
r_joints.header.stamp = ros::Time::now();
|
||||
|
||||
l_arm.header.frame_id = "/odom";
|
||||
l_arm.header.stamp = ros::Time::now();
|
||||
|
||||
r_arm.header.frame_id = "/odom";
|
||||
r_arm.header.stamp = ros::Time::now();
|
||||
|
||||
l_legjoints.header.frame_id = "/odom";
|
||||
l_legjoints.header.stamp = ros::Time::now();
|
||||
|
||||
r_legjoints.header.frame_id = "/odom";
|
||||
r_legjoints.header.stamp = ros::Time::now();
|
||||
|
||||
l_leg.header.frame_id = "/odom";
|
||||
l_leg.header.stamp = ros::Time::now();
|
||||
|
||||
r_leg.header.frame_id = "/odom";
|
||||
r_leg.header.stamp = ros::Time::now();
|
||||
|
||||
camera.header.frame_id = "/odom";
|
||||
camera.header.stamp = ros::Time::now();
|
||||
|
||||
base.header.frame_id = "/odom";
|
||||
base.header.stamp = ros::Time::now();
|
||||
|
||||
// set namespace and id
|
||||
|
||||
body.ns = "body";
|
||||
body.id = 0;
|
||||
|
||||
head.ns = "head";
|
||||
head.id = 1;
|
||||
|
||||
l_shoulder.ns = "l_shoulder";
|
||||
l_shoulder.id = 2;
|
||||
|
||||
r_shoulder.ns = "r_shoulder";
|
||||
r_shoulder.id = 3;
|
||||
|
||||
l_joints.ns = "l_joints";
|
||||
l_joints.id = 4;
|
||||
|
||||
r_joints.ns = "l_joints";
|
||||
r_joints.id = 5;
|
||||
|
||||
l_arm.ns = "l_arm";
|
||||
l_arm.id = 6;
|
||||
|
||||
r_arm.ns = "r_arm";
|
||||
r_arm.id = 7;
|
||||
|
||||
l_legjoints.ns = "l_legjoints";
|
||||
l_legjoints.id = 8;
|
||||
|
||||
r_legjoints.ns = "l_legjoints";
|
||||
r_legjoints.id = 9;
|
||||
|
||||
l_leg.ns = "l_leg";
|
||||
l_leg.id = 10;
|
||||
|
||||
r_leg.ns = "r_leg";
|
||||
r_leg.id = 11;
|
||||
|
||||
camera.ns = "camera";
|
||||
camera.id = 12;
|
||||
|
||||
base.ns = "base";
|
||||
base.id = 13;
|
||||
|
||||
// set marker shape
|
||||
|
||||
body.type = visualization_msgs::Marker::CUBE;
|
||||
head.type = visualization_msgs::Marker::SPHERE;
|
||||
l_shoulder.type = visualization_msgs::Marker::CUBE;
|
||||
r_shoulder.type = visualization_msgs::Marker::CUBE;
|
||||
l_joints.type = visualization_msgs::Marker::POINTS;
|
||||
r_joints.type = visualization_msgs::Marker::POINTS;
|
||||
l_arm.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
r_arm.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
l_legjoints.type = visualization_msgs::Marker::POINTS;
|
||||
r_legjoints.type = visualization_msgs::Marker::POINTS;
|
||||
l_leg.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
r_leg.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
camera.type = visualization_msgs::Marker::SPHERE;
|
||||
base.type = visualization_msgs::Marker::SPHERE;
|
||||
|
||||
// Set the marker action
|
||||
|
||||
body.action = visualization_msgs::Marker::ADD;
|
||||
head.action = visualization_msgs::Marker::ADD;
|
||||
l_shoulder.action = visualization_msgs::Marker::ADD;
|
||||
r_shoulder.action = visualization_msgs::Marker::ADD;
|
||||
l_joints.action = visualization_msgs::Marker::ADD;
|
||||
r_joints.action = visualization_msgs::Marker::ADD;
|
||||
l_arm.action = visualization_msgs::Marker::ADD;
|
||||
r_arm.action = visualization_msgs::Marker::ADD;
|
||||
l_legjoints.action = visualization_msgs::Marker::ADD;
|
||||
r_legjoints.action = visualization_msgs::Marker::ADD;
|
||||
l_leg.action = visualization_msgs::Marker::ADD;
|
||||
r_leg.action = visualization_msgs::Marker::ADD;
|
||||
camera.action = visualization_msgs::Marker::ADD;
|
||||
base.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
// Set the pose of the marker
|
||||
|
||||
body.pose.position.x = torso.x;
|
||||
body.pose.position.y = torso.y;
|
||||
body.pose.position.z = torso.z;
|
||||
body.pose.orientation.x = 0.0;
|
||||
body.pose.orientation.y = 0.0;
|
||||
body.pose.orientation.z = 0.0;
|
||||
body.pose.orientation.w = 1.0;
|
||||
|
||||
head.pose.position.x = torso.x;
|
||||
head.pose.position.y = torso.y;
|
||||
head.pose.position.z = torso.z+0.5;
|
||||
head.pose.orientation.x = 0.0;
|
||||
head.pose.orientation.y = 0.0;
|
||||
head.pose.orientation.z = 0.0;
|
||||
head.pose.orientation.w = 1.0;
|
||||
|
||||
l_shoulder.pose.position.x = torso.x;
|
||||
l_shoulder.pose.position.y = torso.y+0.3;
|
||||
l_shoulder.pose.position.z = torso.z+0.3;
|
||||
l_shoulder.pose.orientation.x = 0.0;
|
||||
l_shoulder.pose.orientation.y = 0.0;
|
||||
l_shoulder.pose.orientation.z = 0.0;
|
||||
l_shoulder.pose.orientation.w = 1.0;
|
||||
|
||||
r_shoulder.pose.position.x = torso.x;
|
||||
r_shoulder.pose.position.y = torso.y-0.3;
|
||||
r_shoulder.pose.position.z = torso.z+0.3;
|
||||
r_shoulder.pose.orientation.x = 0.0;
|
||||
r_shoulder.pose.orientation.y = 0.0;
|
||||
r_shoulder.pose.orientation.z = 0.0;
|
||||
r_shoulder.pose.orientation.w = 1.0;
|
||||
|
||||
l_joints.pose.orientation.w = 1.0;
|
||||
r_joints.pose.orientation.w = 1.0;
|
||||
l_arm.pose.orientation.w = 1.0;
|
||||
r_arm.pose.orientation.w = 1.0;
|
||||
|
||||
l_legjoints.pose.orientation.w = 1.0;
|
||||
r_legjoints.pose.orientation.w = 1.0;
|
||||
l_leg.pose.orientation.w = 1.0;
|
||||
r_leg.pose.orientation.w = 1.0;
|
||||
|
||||
camera.pose.position.x = 0;
|
||||
camera.pose.position.y = 0;
|
||||
camera.pose.position.z = 0.0;
|
||||
camera.pose.orientation.x = 0.0;
|
||||
camera.pose.orientation.y = 0.0;
|
||||
camera.pose.orientation.z = 0.0;
|
||||
camera.pose.orientation.w = 0.0;
|
||||
|
||||
base.pose.position.x = base_position.x;
|
||||
base.pose.position.y = base_position.y;
|
||||
base.pose.position.z = base_position.z;
|
||||
base.pose.orientation.x = 0.0;
|
||||
base.pose.orientation.y = 0.0;
|
||||
base.pose.orientation.z = 0.0;
|
||||
base.pose.orientation.w = 1.0;
|
||||
|
||||
// Set the scale of the marker (in meters)
|
||||
|
||||
body.scale.x = 0.2;
|
||||
body.scale.y = 0.4;
|
||||
body.scale.z = 0.8;
|
||||
|
||||
head.scale.x = 0.3;
|
||||
head.scale.y = 0.3;
|
||||
head.scale.z = 0.4;
|
||||
|
||||
l_leg.scale.x = 0.2;
|
||||
l_leg.scale.y = 0.2;
|
||||
l_leg.scale.z = 0.9;
|
||||
|
||||
r_leg.scale.x = 0.2;
|
||||
r_leg.scale.y = 0.2;
|
||||
r_leg.scale.z = 0.9;
|
||||
|
||||
l_shoulder.scale.x = 0.2;
|
||||
l_shoulder.scale.y = 0.2;
|
||||
l_shoulder.scale.z = 0.2;
|
||||
|
||||
r_shoulder.scale.x = 0.2;
|
||||
r_shoulder.scale.y = 0.2;
|
||||
r_shoulder.scale.z = 0.2;
|
||||
|
||||
l_joints.scale.x = 0.2;
|
||||
l_joints.scale.y = 0.2;
|
||||
|
||||
r_joints.scale.x = 0.2;
|
||||
r_joints.scale.y = 0.2;
|
||||
|
||||
l_arm.scale.x = 0.1;
|
||||
l_arm.scale.y = 0.1;
|
||||
|
||||
r_arm.scale.x = 0.1;
|
||||
r_arm.scale.y = 0.1;
|
||||
|
||||
l_legjoints.scale.x = 0.2;
|
||||
l_legjoints.scale.y = 0.2;
|
||||
|
||||
r_legjoints.scale.x = 0.2;
|
||||
r_legjoints.scale.y = 0.2;
|
||||
|
||||
l_leg.scale.x = 0.2;
|
||||
l_leg.scale.y = 0.2;
|
||||
|
||||
r_leg.scale.x = 0.2;
|
||||
r_leg.scale.y = 0.2;
|
||||
|
||||
camera.scale.x = 0.5;
|
||||
camera.scale.y = 0.5;
|
||||
camera.scale.z = 0.5;
|
||||
|
||||
base.scale.x = base_r;
|
||||
base.scale.y = base_r;
|
||||
base.scale.z = 0.01;
|
||||
|
||||
// Set the color
|
||||
|
||||
body.color.r = 0.0f;
|
||||
body.color.g = 1.0f;
|
||||
body.color.b = 0.5f;
|
||||
body.color.a = 1.0;
|
||||
|
||||
head.color.r = 0.0f;
|
||||
head.color.g = 1.0f;
|
||||
head.color.b = 0.0f;
|
||||
head.color.a = 1.0;
|
||||
|
||||
l_shoulder.color.r = 0.5f;
|
||||
l_shoulder.color.g = 0.2f;
|
||||
l_shoulder.color.b = 1.0f;
|
||||
l_shoulder.color.a = 1.0;
|
||||
|
||||
r_shoulder.color.r = 0.5f;
|
||||
r_shoulder.color.g = 0.2f;
|
||||
r_shoulder.color.b = 1.0f;
|
||||
r_shoulder.color.a = 1.0;
|
||||
|
||||
l_joints.color.r = 0.5f;
|
||||
l_joints.color.g = 0.2f;
|
||||
l_joints.color.b = 1.0f;
|
||||
l_joints.color.a = 1.0;
|
||||
|
||||
r_joints.color.r = 0.5f;
|
||||
r_joints.color.g = 0.2f;
|
||||
r_joints.color.b = 1.0f;
|
||||
r_joints.color.a = 1.0;
|
||||
|
||||
l_arm.color.r = 0.5f;
|
||||
l_arm.color.g = 0.2f;
|
||||
l_arm.color.b = 1.0f;
|
||||
l_arm.color.a = 1.0;
|
||||
|
||||
r_arm.color.r = 0.5f;
|
||||
r_arm.color.g = 0.2f;
|
||||
r_arm.color.b = 1.0f;
|
||||
r_arm.color.a = 1.0;
|
||||
|
||||
l_legjoints.color.r = 0.5f;
|
||||
l_legjoints.color.g = 0.2f;
|
||||
l_legjoints.color.b = 1.0f;
|
||||
l_legjoints.color.a = 1.0;
|
||||
|
||||
r_legjoints.color.r = 0.5f;
|
||||
r_legjoints.color.g = 0.2f;
|
||||
r_legjoints.color.b = 1.0f;
|
||||
r_legjoints.color.a = 1.0;
|
||||
|
||||
l_leg.color.r = 0.5f;
|
||||
l_leg.color.g = 0.2f;
|
||||
l_leg.color.b = 1.0f;
|
||||
l_leg.color.a = 1.0;
|
||||
|
||||
r_leg.color.r = 0.5f;
|
||||
r_leg.color.g = 0.2f;
|
||||
r_leg.color.b = 1.0f;
|
||||
r_leg.color.a = 1.0;
|
||||
|
||||
camera.color.r = 1.0f;
|
||||
camera.color.g = 1.0f;
|
||||
camera.color.b = 1.0f;
|
||||
camera.color.a = 1.0;
|
||||
|
||||
base.color.r = 1.0f;
|
||||
base.color.g = 0.0f;
|
||||
base.color.b = 0.0f;
|
||||
base.color.a = 1.0;
|
||||
|
||||
// set lifetime
|
||||
|
||||
body.lifetime = ros::Duration();
|
||||
head.lifetime = ros::Duration();
|
||||
l_shoulder.lifetime = ros::Duration();
|
||||
r_shoulder.lifetime = ros::Duration();
|
||||
l_joints.lifetime = ros::Duration();
|
||||
r_joints.lifetime = ros::Duration();
|
||||
l_arm.lifetime = ros::Duration();
|
||||
r_arm.lifetime = ros::Duration();
|
||||
l_legjoints.lifetime = ros::Duration();
|
||||
r_legjoints.lifetime = ros::Duration();
|
||||
l_leg.lifetime = ros::Duration();
|
||||
r_leg.lifetime = ros::Duration();
|
||||
camera.lifetime = ros::Duration();
|
||||
base.lifetime = ros::Duration();
|
||||
|
||||
// create lines
|
||||
// create line for arms
|
||||
|
||||
geometry_msgs::Point l_shoulder_point;
|
||||
geometry_msgs::Point r_shoulder_point;
|
||||
|
||||
l_shoulder_point.x = torso.x;
|
||||
l_shoulder_point.y = torso.y+0.3;
|
||||
l_shoulder_point.z = torso.z+0.3;
|
||||
|
||||
r_shoulder_point.x = torso.x;
|
||||
r_shoulder_point.y = torso.y-0.3;
|
||||
r_shoulder_point.z = torso.z+0.3;
|
||||
|
||||
l_joints.points.push_back(l_shoulder_point);
|
||||
l_joints.points.push_back(l_hand);
|
||||
l_arm.points.push_back(l_shoulder_point);
|
||||
l_arm.points.push_back(l_hand);
|
||||
|
||||
r_joints.points.push_back(r_shoulder_point);
|
||||
r_joints.points.push_back(r_hand);
|
||||
r_arm.points.push_back(r_shoulder_point);
|
||||
r_arm.points.push_back(r_hand);
|
||||
|
||||
// create line for legs
|
||||
|
||||
geometry_msgs::Point l_hip;
|
||||
geometry_msgs::Point r_hip;
|
||||
geometry_msgs::Point l_knee;
|
||||
geometry_msgs::Point r_knee;
|
||||
geometry_msgs::Point l_foot;
|
||||
geometry_msgs::Point r_foot;
|
||||
|
||||
l_hip.x = torso.x;
|
||||
l_hip.y = torso.y+0.2;
|
||||
l_hip.z = torso.z-0.15;
|
||||
|
||||
r_hip.x = torso.x;
|
||||
r_hip.y = torso.y-0.2;
|
||||
r_hip.z = torso.z-0.15;
|
||||
|
||||
l_knee.x = torso.x;
|
||||
l_knee.y = torso.y+0.2;
|
||||
l_knee.z = torso.z-0.55;
|
||||
|
||||
r_knee.x = torso.x;
|
||||
r_knee.y = torso.y-0.2;
|
||||
r_knee.z = torso.z-0.55;
|
||||
|
||||
l_foot.x = torso.x;
|
||||
l_foot.y = torso.y+0.2;
|
||||
l_foot.z = torso.z-0.95;
|
||||
|
||||
r_foot.x = torso.x;
|
||||
r_foot.y = torso.y-0.2;
|
||||
r_foot.z = torso.z-0.95;
|
||||
|
||||
// push to points and lines
|
||||
|
||||
l_legjoints.points.push_back(l_hip);
|
||||
l_legjoints.points.push_back(l_knee);
|
||||
l_legjoints.points.push_back(l_foot);
|
||||
|
||||
l_leg.points.push_back(l_hip);
|
||||
l_leg.points.push_back(l_knee);
|
||||
l_leg.points.push_back(l_foot);
|
||||
|
||||
r_legjoints.points.push_back(r_hip);
|
||||
r_legjoints.points.push_back(r_knee);
|
||||
r_legjoints.points.push_back(r_foot);
|
||||
|
||||
r_leg.points.push_back(r_hip);
|
||||
r_leg.points.push_back(r_knee);
|
||||
r_leg.points.push_back(r_foot);
|
||||
|
||||
|
||||
// Publish the markers
|
||||
|
||||
while (marker_pub.getNumSubscribers() < 1)
|
||||
{
|
||||
if (!ros::ok())
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
ROS_WARN_ONCE("Please create a subscriber to the marker");
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
marker_pub.publish(body);
|
||||
marker_pub.publish(head);
|
||||
marker_pub.publish(l_shoulder);
|
||||
marker_pub.publish(r_shoulder);
|
||||
marker_pub.publish(l_joints);
|
||||
marker_pub.publish(r_joints);
|
||||
marker_pub.publish(l_arm);
|
||||
marker_pub.publish(r_arm);
|
||||
marker_pub.publish(l_legjoints);
|
||||
marker_pub.publish(r_legjoints);
|
||||
marker_pub.publish(l_leg);
|
||||
marker_pub.publish(r_leg);
|
||||
marker_pub.publish(camera);
|
||||
marker_pub.publish(base);
|
||||
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
|
||||
// move hole human
|
||||
i++;
|
||||
|
||||
// publish markers
|
||||
|
||||
body.action = visualization_msgs::Marker::ADD;
|
||||
head.action = visualization_msgs::Marker::ADD;
|
||||
l_arm.action = visualization_msgs::Marker::ADD;
|
||||
r_arm.action = visualization_msgs::Marker::ADD;
|
||||
l_leg.action = visualization_msgs::Marker::ADD;
|
||||
r_arm.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
marker_pub.publish(body);
|
||||
marker_pub.publish(head);
|
||||
marker_pub.publish(l_leg);
|
||||
marker_pub.publish(r_leg);
|
||||
marker_pub.publish(l_arm);
|
||||
marker_pub.publish(r_arm);
|
||||
marker_pub.publish(camera);
|
||||
i = i +1;
|
||||
|
||||
r.sleep();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
quarternion toQuaternion( double yaw, double pitch, double roll) {
|
||||
// Abbreviations for the various angular functions
|
||||
double cy = cos(yaw * 0.5);
|
||||
double sy = sin(yaw * 0.5);
|
||||
double cp = cos(pitch * 0.5);
|
||||
double sp = sin(pitch * 0.5);
|
||||
double cr = cos(roll * 0.5);
|
||||
double sr = sin(roll * 0.5);
|
||||
|
||||
quarternion q;
|
||||
|
||||
q.x = cy * cp * sr - sy * sp * cr;
|
||||
q.y = sy * cp * sr + cy * sp * cr;
|
||||
q.z = sy * cp * cr - cy * sp * sr;
|
||||
q.w = cy * cp * cr + sy * sp * sr;
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user