refactored the code, fixed config bug
This commit is contained in:
Pavel Lutskov
@@ -6,7 +6,7 @@
|
|||||||
"lt": 0.5666157603263855,
|
"lt": 0.5666157603263855,
|
||||||
"lr": 0.6060229593671598,
|
"lr": 0.6060229593671598,
|
||||||
"cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864],
|
"cr": [-1.9731173515319824, -0.04246790334582329, -0.050492866697747864],
|
||||||
"arm": 0.66392470071039278
|
"arm": 0.66392470071039278,
|
||||||
"should": {
|
"should": {
|
||||||
"L": [0, 0.08, 0.15],
|
"L": [0, 0.08, 0.15],
|
||||||
"R": [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"/>
|
output="screen"/>
|
||||||
<node name="walker" pkg="teleoperation" type="walker.py"/>
|
<node name="walker" pkg="teleoperation" type="walker.py"/>
|
||||||
<node name="fall_detector" pkg="teleoperation" type="fall_detector.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>
|
</launch>
|
||||||
|
|||||||
@@ -7,11 +7,10 @@ Panels:
|
|||||||
- /Global Options1
|
- /Global Options1
|
||||||
- /Status1
|
- /Status1
|
||||||
- /Grid1
|
- /Grid1
|
||||||
- /Grid1/Offset1
|
|
||||||
- /TF1
|
- /TF1
|
||||||
- /TF1/Frames1
|
- /TF1/Frames1
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
Tree Height: 359
|
Tree Height: 775
|
||||||
- Class: rviz/Selection
|
- Class: rviz/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz/Tool Properties
|
- Class: rviz/Tool Properties
|
||||||
@@ -47,340 +46,48 @@ Visualization Manager:
|
|||||||
Offset:
|
Offset:
|
||||||
X: 0
|
X: 0
|
||||||
Y: 0
|
Y: 0
|
||||||
Z: 0
|
Z: -1
|
||||||
Plane: XY
|
Plane: XY
|
||||||
Plane Cell Count: 10
|
Plane Cell Count: 10
|
||||||
Reference Frame: odom
|
Reference Frame: <Fixed Frame>
|
||||||
Value: true
|
Value: true
|
||||||
- Class: rviz/TF
|
- Class: rviz/TF
|
||||||
Enabled: true
|
Enabled: false
|
||||||
Frame Timeout: 15
|
Frame Timeout: 15
|
||||||
Frames:
|
Frames:
|
||||||
All Enabled: false
|
All Enabled: false
|
||||||
Aruco_0_frame:
|
Marker Scale: 0.2
|
||||||
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
|
|
||||||
Name: TF
|
Name: TF
|
||||||
Show Arrows: true
|
Show Arrows: true
|
||||||
Show Axes: false
|
Show Axes: true
|
||||||
Show Names: true
|
Show Names: true
|
||||||
Tree:
|
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
|
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
|
Value: true
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 48; 48; 48
|
Background Color: 48; 48; 48
|
||||||
Fixed Frame: torso
|
Fixed Frame: odom
|
||||||
Frame Rate: 30
|
Frame Rate: 30
|
||||||
Name: root
|
Name: root
|
||||||
Tools:
|
Tools:
|
||||||
@@ -401,30 +108,30 @@ Visualization Manager:
|
|||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz/Orbit
|
Class: rviz/Orbit
|
||||||
Distance: 1.25714
|
Distance: 8.05248
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.06
|
Stereo Eye Separation: 0.06
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
Swap Stereo Eyes: false
|
Swap Stereo Eyes: false
|
||||||
Value: false
|
Value: false
|
||||||
Focal Point:
|
Focal Point:
|
||||||
X: 0
|
X: -0.098179
|
||||||
Y: 0
|
Y: 0.0497635
|
||||||
Z: 0
|
Z: 0.136802
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.01
|
Near Clip Distance: 0.01
|
||||||
Pitch: 0.280398
|
Pitch: 0.3654
|
||||||
Target Frame: <Fixed Frame>
|
Target Frame: <Fixed Frame>
|
||||||
Value: Orbit (rviz)
|
Value: Orbit (rviz)
|
||||||
Yaw: 6.11041
|
Yaw: 3.14176
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 663
|
Height: 1056
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: false
|
||||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000279fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000000000000279000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002f80000027900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Time:
|
Time:
|
||||||
@@ -433,6 +140,6 @@ Window Geometry:
|
|||||||
collapsed: false
|
collapsed: false
|
||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 760
|
Width: 1855
|
||||||
X: 1015
|
X: 65
|
||||||
Y: 14
|
Y: 274
|
||||||
|
|||||||
@@ -7,10 +7,10 @@ Panels:
|
|||||||
- /Global Options1
|
- /Global Options1
|
||||||
- /Status1
|
- /Status1
|
||||||
- /Grid1
|
- /Grid1
|
||||||
- /Grid1/Offset1
|
- /RobotModel1
|
||||||
- /TF1
|
- /TF1
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
Tree Height: 286
|
Tree Height: 895
|
||||||
- Class: rviz/Selection
|
- Class: rviz/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz/Tool Properties
|
- Class: rviz/Tool Properties
|
||||||
@@ -51,320 +51,415 @@ Visualization Manager:
|
|||||||
Plane Cell Count: 10
|
Plane Cell Count: 10
|
||||||
Reference Frame: odom
|
Reference Frame: odom
|
||||||
Value: true
|
Value: true
|
||||||
- Class: rviz/TF
|
- Alpha: 1
|
||||||
|
Class: rviz/RobotModel
|
||||||
|
Collision Enabled: false
|
||||||
Enabled: true
|
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
|
Frame Timeout: 15
|
||||||
Frames:
|
Frames:
|
||||||
All Enabled: true
|
All Enabled: true
|
||||||
CameraBottom_frame:
|
Marker Scale: 0.3
|
||||||
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
|
|
||||||
Name: TF
|
Name: TF
|
||||||
Show Arrows: true
|
Show Arrows: true
|
||||||
Show Axes: false
|
Show Axes: true
|
||||||
Show Names: false
|
Show Names: true
|
||||||
Tree:
|
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
|
Update Interval: 0
|
||||||
Value: true
|
Value: false
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 48; 48; 48
|
Background Color: 48; 48; 48
|
||||||
@@ -389,7 +484,7 @@ Visualization Manager:
|
|||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz/Orbit
|
Class: rviz/Orbit
|
||||||
Distance: 1.11097
|
Distance: 1.26786
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.06
|
Stereo Eye Separation: 0.06
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
@@ -401,18 +496,18 @@ Visualization Manager:
|
|||||||
Z: 0
|
Z: 0
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.01
|
Near Clip Distance: 0.01
|
||||||
Pitch: 0.300398
|
Pitch: 0.574797
|
||||||
Target Frame: <Fixed Frame>
|
Target Frame: <Fixed Frame>
|
||||||
Value: Orbit (rviz)
|
Value: Orbit (rviz)
|
||||||
Yaw: 3.1404
|
Yaw: 3.1104
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 459
|
Height: 1176
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: false
|
||||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000001adfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000000000001ad000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000019b000001ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000040efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000040e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000040e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Time:
|
Time:
|
||||||
@@ -421,6 +516,6 @@ Window Geometry:
|
|||||||
collapsed: false
|
collapsed: false
|
||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 411
|
Width: 1855
|
||||||
X: 55
|
X: 1985
|
||||||
Y: 14
|
Y: 24
|
||||||
|
|||||||
@@ -5,12 +5,12 @@ import json
|
|||||||
from math import asin, atan, radians
|
from math import asin, atan, radians
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from naoqi import ALProxy
|
|
||||||
|
from masterloop import mp
|
||||||
|
|
||||||
|
|
||||||
FRAME_TORSO = 0
|
FRAME_TORSO = 0
|
||||||
K = 0.1
|
K = 0.1
|
||||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
|
||||||
|
|
||||||
_here = os.path.dirname(os.path.realpath(__file__))
|
_here = os.path.dirname(os.path.realpath(__file__))
|
||||||
with open('{}/../config/default.yaml'.format(_here)) as f:
|
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)
|
nao_xyz = to_nao(my_xyz, side)
|
||||||
delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
|
delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
|
||||||
crt_q = mp.getAngles([side + j for j in JOINTS], False)
|
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
|
return crt_q + delta_q
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -4,7 +4,7 @@ import os
|
|||||||
import rospy
|
import rospy
|
||||||
from naoqi import ALProxy, ALBroker, ALModule
|
from naoqi import ALProxy, ALBroker, ALModule
|
||||||
|
|
||||||
from masterloop import inform_masterloop_factory
|
from masterloop import inform_masterloop_factory, mp
|
||||||
|
|
||||||
|
|
||||||
fall_broker = None
|
fall_broker = None
|
||||||
@@ -18,7 +18,6 @@ class FallDetectorModule(ALModule):
|
|||||||
|
|
||||||
def __init__(self, name):
|
def __init__(self, name):
|
||||||
ALModule.__init__(self, name)
|
ALModule.__init__(self, name)
|
||||||
self.mp = ALProxy('ALMotion')
|
|
||||||
|
|
||||||
def on_robot_falling(self, *_args):
|
def on_robot_falling(self, *_args):
|
||||||
_inform_masterloop('falling')
|
_inform_masterloop('falling')
|
||||||
@@ -26,9 +25,11 @@ class FallDetectorModule(ALModule):
|
|||||||
def on_robot_fallen(self, *_args):
|
def on_robot_fallen(self, *_args):
|
||||||
if not _inform_masterloop('fallen'):
|
if not _inform_masterloop('fallen'):
|
||||||
return
|
return
|
||||||
self.mp.rest()
|
mp.rest()
|
||||||
rospy.Rate(0.5).sleep()
|
rospy.Rate(0.5).sleep()
|
||||||
self.mp.wakeUp()
|
mp.wakeUp()
|
||||||
|
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
||||||
|
am.setExpressiveListeningEnabled(False)
|
||||||
pp = ALProxy('ALRobotPosture')
|
pp = ALProxy('ALRobotPosture')
|
||||||
while not pp.goToPosture('StandInit', 1.0):
|
while not pp.goToPosture('StandInit', 1.0):
|
||||||
pass
|
pass
|
||||||
|
|||||||
@@ -1,12 +1,10 @@
|
|||||||
#! /usr/bin/env python
|
#! /usr/bin/env python
|
||||||
import os
|
|
||||||
|
|
||||||
import rospy
|
import rospy
|
||||||
import tf
|
import tf
|
||||||
from naoqi import ALProxy
|
|
||||||
|
|
||||||
from masterloop import inform_masterloop_factory
|
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')
|
_inform_masterloop = inform_masterloop_factory('imitator')
|
||||||
@@ -21,27 +19,22 @@ if __name__ == '__main__':
|
|||||||
|
|
||||||
ll = tf.TransformListener()
|
ll = tf.TransformListener()
|
||||||
|
|
||||||
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
|
||||||
am.setExpressiveListeningEnabled(False)
|
|
||||||
|
|
||||||
mp.wakeUp()
|
|
||||||
|
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
rospy.Rate(10).sleep()
|
rospy.Rate(10).sleep()
|
||||||
if not _inform_masterloop('imitate'):
|
if not _inform_masterloop('imitate'):
|
||||||
continue
|
continue
|
||||||
rospy.logdebug('IMITATOR: ACTIVE')
|
rospy.logdebug('IMITATOR: ACTIVE')
|
||||||
|
|
||||||
if TORSO:
|
# if TORSO:
|
||||||
try:
|
# try:
|
||||||
_, q = ll.lookupTransform('odom',
|
# _, q = ll.lookupTransform('odom',
|
||||||
'Aruco_0_frame',
|
# 'Aruco_0_frame',
|
||||||
rospy.Time(0))
|
# rospy.Time(0))
|
||||||
rot = tf.transformations.euler_from_quaternion(q)
|
# rot = tf.transformations.euler_from_quaternion(q)
|
||||||
mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
|
# mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
|
||||||
[-rot[1], -rot[1]], 0.3)
|
# [-rot[1], -rot[1]], 0.3)
|
||||||
except Exception as e:
|
# except Exception as e:
|
||||||
rospy.logwarn(e)
|
# rospy.logwarn(e)
|
||||||
|
|
||||||
for i, side in enumerate(['L', 'R'], 1):
|
for i, side in enumerate(['L', 'R'], 1):
|
||||||
try:
|
try:
|
||||||
@@ -54,6 +47,4 @@ if __name__ == '__main__':
|
|||||||
rospy.logwarn(e)
|
rospy.logwarn(e)
|
||||||
continue
|
continue
|
||||||
|
|
||||||
movement(my_arm_xyz, side, dumb)
|
movement(my_arm_xyz, side, our_cartesian)
|
||||||
|
|
||||||
mp.rest()
|
|
||||||
|
|||||||
@@ -1,4 +1,5 @@
|
|||||||
#! /usr/bin/env python
|
#! /usr/bin/env python
|
||||||
|
import os
|
||||||
from argparse import ArgumentParser
|
from argparse import ArgumentParser
|
||||||
|
|
||||||
import rospy
|
import rospy
|
||||||
@@ -7,6 +8,7 @@ import actionlib
|
|||||||
from teleoperation.srv import InformMasterloop, InformMasterloopResponse
|
from teleoperation.srv import InformMasterloop, InformMasterloopResponse
|
||||||
from teleoperation.srv import Hands
|
from teleoperation.srv import Hands
|
||||||
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
|
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
|
||||||
|
from naoqi import ALProxy
|
||||||
|
|
||||||
|
|
||||||
# Constants
|
# Constants
|
||||||
@@ -32,6 +34,8 @@ state = 'dead' # Also walk, imitate, fallen, idle
|
|||||||
hand_state = 'closed'
|
hand_state = 'closed'
|
||||||
speech_in_progress = False
|
speech_in_progress = False
|
||||||
|
|
||||||
|
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
||||||
|
|
||||||
|
|
||||||
def init_voc_state_speech():
|
def init_voc_state_speech():
|
||||||
global VOC_STATE, SPEECH_TRANSITIONS
|
global VOC_STATE, SPEECH_TRANSITIONS
|
||||||
@@ -144,15 +148,24 @@ if __name__ == '__main__':
|
|||||||
AI = args.autoimitate
|
AI = args.autoimitate
|
||||||
init_voc_state_speech()
|
init_voc_state_speech()
|
||||||
|
|
||||||
ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
|
|
||||||
|
|
||||||
speech = actionlib.SimpleActionClient('speech_server',
|
speech = actionlib.SimpleActionClient('speech_server',
|
||||||
RequestSpeechAction)
|
RequestSpeechAction)
|
||||||
|
|
||||||
speech.wait_for_server()
|
speech.wait_for_server()
|
||||||
rospy.loginfo('SPEECH: SERVER THERE')
|
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():
|
while not rospy.is_shutdown():
|
||||||
if state in ('idle', 'imitate', 'dead'):
|
if state in ('idle', 'imitate', 'dead'):
|
||||||
|
|||||||
@@ -5,9 +5,8 @@ from time import sleep
|
|||||||
|
|
||||||
import rospy
|
import rospy
|
||||||
import tf
|
import tf
|
||||||
from naoqi import ALProxy
|
|
||||||
|
|
||||||
from masterloop import inform_masterloop_factory
|
from masterloop import inform_masterloop_factory, mp
|
||||||
|
|
||||||
|
|
||||||
FW = None
|
FW = None
|
||||||
@@ -60,9 +59,6 @@ if __name__ == '__main__':
|
|||||||
global_init()
|
global_init()
|
||||||
rospy.wait_for_service('inform_masterloop')
|
rospy.wait_for_service('inform_masterloop')
|
||||||
ll = tf.TransformListener()
|
ll = tf.TransformListener()
|
||||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
|
||||||
mp.wakeUp()
|
|
||||||
mp.moveInit()
|
|
||||||
mp.setMoveArmsEnabled(False, False)
|
mp.setMoveArmsEnabled(False, False)
|
||||||
|
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
|
|||||||
@@ -4,47 +4,43 @@
|
|||||||
#include <visualization_msgs/Marker.h>
|
#include <visualization_msgs/Marker.h>
|
||||||
#include <tf/transform_listener.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 )
|
int main( int argc, char** argv )
|
||||||
{
|
{
|
||||||
ros::init(argc, argv, "rviz_human");
|
ros::init(argc, argv, "rviz_human");
|
||||||
ros::NodeHandle n;
|
ros::NodeHandle n;
|
||||||
ros::Rate r(100);
|
ros::Rate r(100);
|
||||||
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
|
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
|
||||||
"visualization_marker", 10
|
"visualization_marker", 15);
|
||||||
);
|
|
||||||
|
|
||||||
tf::TransformListener tll;
|
tf::TransformListener aruco_0;
|
||||||
|
tf::TransformListener aruco_1;
|
||||||
|
tf::TransformListener aruco_2;
|
||||||
|
|
||||||
int i = 0;
|
// suppose we already have the position of the center marker in the correct frame
|
||||||
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
|
geometry_msgs::Point torso;
|
||||||
// in the correct frame
|
geometry_msgs::Point l_hand;
|
||||||
|
geometry_msgs::Point r_hand;
|
||||||
|
geometry_msgs::Point base_position;
|
||||||
|
|
||||||
float x_start = 2;
|
// Set human to starting postion
|
||||||
float y_start = 0;
|
torso.x = -1;
|
||||||
float z_start = 1.3;
|
torso.y = 0;
|
||||||
|
torso.z = 0;
|
||||||
|
|
||||||
float x_0 = 2;
|
l_hand.x = -1;
|
||||||
float y_0 = 0;
|
l_hand.y = 0.35;
|
||||||
float z_0 = 1.3;
|
l_hand.z = -0.2;
|
||||||
|
|
||||||
float x_1 = 0;
|
r_hand.x = -1;
|
||||||
float y_1 = 0;
|
r_hand.y = -0.35;
|
||||||
float z_1 = 0;
|
r_hand.z = -0.2;
|
||||||
|
|
||||||
|
base_position.x = -1;
|
||||||
|
base_position.y = 0;
|
||||||
|
base_position.z = -1;
|
||||||
|
|
||||||
|
double base_r = 2;
|
||||||
|
|
||||||
while (ros::ok())
|
while (ros::ok())
|
||||||
{
|
{
|
||||||
@@ -54,68 +50,75 @@ int main( int argc, char** argv )
|
|||||||
tf::StampedTransform aruco_1_tf;
|
tf::StampedTransform aruco_1_tf;
|
||||||
tf::StampedTransform aruco_2_tf;
|
tf::StampedTransform aruco_2_tf;
|
||||||
|
|
||||||
try {
|
try{
|
||||||
tll.lookupTransform(
|
aruco_0.lookupTransform("/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
|
||||||
"/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
|
ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
|
||||||
/*ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
|
|
||||||
ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
|
ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
|
||||||
ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
|
ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
|
||||||
*/
|
|
||||||
|
|
||||||
tll.lookupTransform(
|
aruco_1.lookupTransform("/odom", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
|
||||||
"/Aruco_0_frame", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
|
|
||||||
/*
|
|
||||||
ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
|
ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
|
||||||
ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
|
ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
|
||||||
ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
|
ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
|
||||||
*/
|
|
||||||
|
|
||||||
x_0 = aruco_0_tf.getOrigin()[0];
|
aruco_2.lookupTransform("/odom", "/Aruco_2_frame",ros::Time(0), aruco_2_tf);
|
||||||
y_0 = aruco_0_tf.getOrigin()[1];
|
ROS_INFO("0_2: %f",aruco_2_tf.getOrigin()[0]);
|
||||||
z_0 = aruco_0_tf.getOrigin()[2];
|
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];
|
||||||
|
|
||||||
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){
|
catch (tf::TransformException ex){
|
||||||
ROS_ERROR("%s",ex.what());
|
ROS_INFO("%s",ex.what());
|
||||||
ros::Duration(1.0).sleep();
|
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 body;
|
||||||
visualization_msgs::Marker head;
|
visualization_msgs::Marker head;
|
||||||
visualization_msgs::Marker l_leg;
|
visualization_msgs::Marker l_shoulder;
|
||||||
visualization_msgs::Marker r_leg;
|
visualization_msgs::Marker r_shoulder;
|
||||||
|
visualization_msgs::Marker l_joints;
|
||||||
|
visualization_msgs::Marker r_joints;
|
||||||
visualization_msgs::Marker l_arm;
|
visualization_msgs::Marker l_arm;
|
||||||
visualization_msgs::Marker r_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 camera;
|
||||||
|
visualization_msgs::Marker base;
|
||||||
|
|
||||||
|
// Set the frame ID and timestamp
|
||||||
|
|
||||||
// Set the frame ID and timestamp.
|
|
||||||
// See the TF tutorials for information on these.
|
|
||||||
body.header.frame_id = "/odom";
|
body.header.frame_id = "/odom";
|
||||||
body.header.stamp = ros::Time::now();
|
body.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
head.header.frame_id = "/odom";
|
head.header.frame_id = "/odom";
|
||||||
head.header.stamp = ros::Time::now();
|
head.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
l_leg.header.frame_id = "/odom";
|
l_shoulder.header.frame_id = "/odom";
|
||||||
l_leg.header.stamp = ros::Time::now();
|
l_shoulder.header.stamp = ros::Time::now();
|
||||||
|
|
||||||
r_leg.header.frame_id = "/odom";
|
r_shoulder.header.frame_id = "/odom";
|
||||||
r_leg.header.stamp = ros::Time::now();
|
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.frame_id = "/odom";
|
||||||
l_arm.header.stamp = ros::Time::now();
|
l_arm.header.stamp = ros::Time::now();
|
||||||
@@ -123,129 +126,163 @@ int main( int argc, char** argv )
|
|||||||
r_arm.header.frame_id = "/odom";
|
r_arm.header.frame_id = "/odom";
|
||||||
r_arm.header.stamp = ros::Time::now();
|
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.frame_id = "/odom";
|
||||||
camera.header.stamp = ros::Time::now();
|
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.ns = "body";
|
||||||
body.id = 0;
|
body.id = 0;
|
||||||
|
|
||||||
head.ns = "head";
|
head.ns = "head";
|
||||||
head.id = 1;
|
head.id = 1;
|
||||||
|
|
||||||
l_leg.ns = "l_leg";
|
l_shoulder.ns = "l_shoulder";
|
||||||
l_leg.id = 2;
|
l_shoulder.id = 2;
|
||||||
|
|
||||||
r_leg.ns = "r_leg";
|
r_shoulder.ns = "r_shoulder";
|
||||||
r_leg.id = 3;
|
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.ns = "l_arm";
|
||||||
l_arm.id = 4;
|
l_arm.id = 6;
|
||||||
|
|
||||||
r_arm.ns = "r_arm";
|
r_arm.ns = "r_arm";
|
||||||
r_arm.id = 5;
|
r_arm.id = 7;
|
||||||
|
|
||||||
camera.ns = "r_arm";
|
l_legjoints.ns = "l_legjoints";
|
||||||
camera.id = 6;
|
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;
|
body.type = visualization_msgs::Marker::CUBE;
|
||||||
head.type = visualization_msgs::Marker::SPHERE;
|
head.type = visualization_msgs::Marker::SPHERE;
|
||||||
l_leg.type = visualization_msgs::Marker::CYLINDER;
|
l_shoulder.type = visualization_msgs::Marker::CUBE;
|
||||||
r_leg.type = visualization_msgs::Marker::CYLINDER;
|
r_shoulder.type = visualization_msgs::Marker::CUBE;
|
||||||
l_arm.type = visualization_msgs::Marker::CYLINDER;
|
l_joints.type = visualization_msgs::Marker::POINTS;
|
||||||
r_arm.type = visualization_msgs::Marker::CYLINDER;
|
r_joints.type = visualization_msgs::Marker::POINTS;
|
||||||
camera.type = visualization_msgs::Marker::CUBE;
|
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.
|
// Set the marker action
|
||||||
// Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
|
|
||||||
|
|
||||||
body.action = visualization_msgs::Marker::ADD;
|
body.action = visualization_msgs::Marker::ADD;
|
||||||
head.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;
|
l_arm.action = visualization_msgs::Marker::ADD;
|
||||||
r_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;
|
l_leg.action = visualization_msgs::Marker::ADD;
|
||||||
r_arm.action = visualization_msgs::Marker::ADD;
|
r_leg.action = visualization_msgs::Marker::ADD;
|
||||||
camera.action = visualization_msgs::Marker::ADD;
|
camera.action = visualization_msgs::Marker::ADD;
|
||||||
|
base.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
// Set the pose of the marker
|
||||||
|
|
||||||
// Set the pose of the marker.
|
body.pose.position.x = torso.x;
|
||||||
// This is a full 6DOF pose relative to the
|
body.pose.position.y = torso.y;
|
||||||
// frame/time specified in the header
|
body.pose.position.z = torso.z;
|
||||||
|
|
||||||
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.x = 0.0;
|
||||||
body.pose.orientation.y = 0.0;
|
body.pose.orientation.y = 0.0;
|
||||||
body.pose.orientation.z = 0.0;
|
body.pose.orientation.z = 0.0;
|
||||||
body.pose.orientation.w = 1.0;
|
body.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
head.pose.position.x = x_0;
|
head.pose.position.x = torso.x;
|
||||||
head.pose.position.y = y_0;
|
head.pose.position.y = torso.y;
|
||||||
head.pose.position.z = z_0+0.5; //1.85
|
head.pose.position.z = torso.z+0.5;
|
||||||
head.pose.orientation.x = 0.0;
|
head.pose.orientation.x = 0.0;
|
||||||
head.pose.orientation.y = 0.0;
|
head.pose.orientation.y = 0.0;
|
||||||
head.pose.orientation.z = 0.0;
|
head.pose.orientation.z = 0.0;
|
||||||
head.pose.orientation.w = 1.0;
|
head.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
l_leg.pose.position.x = x_0;
|
l_shoulder.pose.position.x = torso.x;
|
||||||
l_leg.pose.position.y = y_0+0.2;
|
l_shoulder.pose.position.y = torso.y+0.3;
|
||||||
l_leg.pose.position.z = z_0-0.8; //0.4
|
l_shoulder.pose.position.z = torso.z+0.3;
|
||||||
l_leg.pose.orientation.x = 0.0;
|
l_shoulder.pose.orientation.x = 0.0;
|
||||||
l_leg.pose.orientation.y = l_leg_y;
|
l_shoulder.pose.orientation.y = 0.0;
|
||||||
l_leg.pose.orientation.z = 0.0;
|
l_shoulder.pose.orientation.z = 0.0;
|
||||||
l_leg.pose.orientation.w = 1.0;
|
l_shoulder.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
r_leg.pose.position.x = x_0;
|
r_shoulder.pose.position.x = torso.x;
|
||||||
r_leg.pose.position.y = y_0-0.2;
|
r_shoulder.pose.position.y = torso.y-0.3;
|
||||||
r_leg.pose.position.z = z_0-0.8; //0.4
|
r_shoulder.pose.position.z = torso.z+0.3;
|
||||||
r_leg.pose.orientation.x = 0.0;
|
r_shoulder.pose.orientation.x = 0.0;
|
||||||
r_leg.pose.orientation.y = r_leg_y;
|
r_shoulder.pose.orientation.y = 0.0;
|
||||||
r_leg.pose.orientation.z = 0.0;
|
r_shoulder.pose.orientation.z = 0.0;
|
||||||
r_leg.pose.orientation.w = 1.0;
|
r_shoulder.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
// calculate left arm angel in z y plane; arm length is 0.5
|
l_joints.pose.orientation.w = 1.0;
|
||||||
|
r_joints.pose.orientation.w = 1.0;
|
||||||
float alpha = atan(z_1/y_1);
|
l_arm.pose.orientation.w = 1.0;
|
||||||
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;
|
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.x = 0;
|
||||||
camera.pose.position.y = 0;
|
camera.pose.position.y = 0;
|
||||||
camera.pose.position.z = 0.5;
|
camera.pose.position.z = 0.0;
|
||||||
camera.pose.orientation.x = 0.0;
|
camera.pose.orientation.x = 0.0;
|
||||||
camera.pose.orientation.y = 0.0;
|
camera.pose.orientation.y = 0.0;
|
||||||
camera.pose.orientation.z = 0.0;
|
camera.pose.orientation.z = 0.0;
|
||||||
camera.pose.orientation.w = 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 -- 1x1x1 here means 1m on a side
|
// Set the scale of the marker (in meters)
|
||||||
|
|
||||||
body.scale.x = 0.2;
|
body.scale.x = 0.2;
|
||||||
body.scale.y = 0.4;
|
body.scale.y = 0.4;
|
||||||
@@ -263,19 +300,47 @@ int main( int argc, char** argv )
|
|||||||
r_leg.scale.y = 0.2;
|
r_leg.scale.y = 0.2;
|
||||||
r_leg.scale.z = 0.9;
|
r_leg.scale.z = 0.9;
|
||||||
|
|
||||||
l_arm.scale.x = 0.2;
|
l_shoulder.scale.x = 0.2;
|
||||||
l_arm.scale.y = 0.2;
|
l_shoulder.scale.y = 0.2;
|
||||||
l_arm.scale.z = 0.5;
|
l_shoulder.scale.z = 0.2;
|
||||||
|
|
||||||
r_arm.scale.x = 0.2;
|
r_shoulder.scale.x = 0.2;
|
||||||
r_arm.scale.y = 0.2;
|
r_shoulder.scale.y = 0.2;
|
||||||
r_arm.scale.z = 0.5;
|
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.x = 0.5;
|
||||||
camera.scale.y = 0.5;
|
camera.scale.y = 0.5;
|
||||||
camera.scale.z = 1;
|
camera.scale.z = 0.5;
|
||||||
|
|
||||||
// Set the color -- be sure to set alpha to something non-zero!
|
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.r = 0.0f;
|
||||||
body.color.g = 1.0f;
|
body.color.g = 1.0f;
|
||||||
@@ -287,42 +352,160 @@ int main( int argc, char** argv )
|
|||||||
head.color.b = 0.0f;
|
head.color.b = 0.0f;
|
||||||
head.color.a = 1.0;
|
head.color.a = 1.0;
|
||||||
|
|
||||||
l_leg.color.r = 1.0f;
|
l_shoulder.color.r = 0.5f;
|
||||||
l_leg.color.g = 0.5f;
|
l_shoulder.color.g = 0.2f;
|
||||||
l_leg.color.b = 0.0f;
|
l_shoulder.color.b = 1.0f;
|
||||||
l_leg.color.a = 1.0;
|
l_shoulder.color.a = 1.0;
|
||||||
|
|
||||||
r_leg.color.r = 1.0f;
|
r_shoulder.color.r = 0.5f;
|
||||||
r_leg.color.g = 0.5f;
|
r_shoulder.color.g = 0.2f;
|
||||||
r_leg.color.b = 0.0f;
|
r_shoulder.color.b = 1.0f;
|
||||||
r_leg.color.a = 1.0;
|
r_shoulder.color.a = 1.0;
|
||||||
|
|
||||||
l_arm.color.r = 1.0f;
|
l_joints.color.r = 0.5f;
|
||||||
l_arm.color.g = 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.b = 1.0f;
|
||||||
l_arm.color.a = 1.0;
|
l_arm.color.a = 1.0;
|
||||||
|
|
||||||
r_arm.color.r = 1.0f;
|
r_arm.color.r = 0.5f;
|
||||||
r_arm.color.g = 0.5f;
|
r_arm.color.g = 0.2f;
|
||||||
r_arm.color.b = 1.0f;
|
r_arm.color.b = 1.0f;
|
||||||
r_arm.color.a = 1.0;
|
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.r = 1.0f;
|
||||||
camera.color.g = 1.0f;
|
camera.color.g = 1.0f;
|
||||||
camera.color.b = 1.0f;
|
camera.color.b = 1.0f;
|
||||||
camera.color.a = 1.0;
|
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();
|
body.lifetime = ros::Duration();
|
||||||
head.lifetime = ros::Duration();
|
head.lifetime = ros::Duration();
|
||||||
l_leg.lifetime = ros::Duration();
|
l_shoulder.lifetime = ros::Duration();
|
||||||
r_leg.lifetime = ros::Duration();
|
r_shoulder.lifetime = ros::Duration();
|
||||||
|
l_joints.lifetime = ros::Duration();
|
||||||
|
r_joints.lifetime = ros::Duration();
|
||||||
l_arm.lifetime = ros::Duration();
|
l_arm.lifetime = ros::Duration();
|
||||||
r_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();
|
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 marker
|
// Publish the markers
|
||||||
|
|
||||||
while (marker_pub.getNumSubscribers() < 1)
|
while (marker_pub.getNumSubscribers() < 1)
|
||||||
{
|
{
|
||||||
@@ -334,71 +517,21 @@ int main( int argc, char** argv )
|
|||||||
sleep(1);
|
sleep(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// 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(body);
|
||||||
marker_pub.publish(head);
|
marker_pub.publish(head);
|
||||||
marker_pub.publish(l_leg);
|
marker_pub.publish(l_shoulder);
|
||||||
marker_pub.publish(r_leg);
|
marker_pub.publish(r_shoulder);
|
||||||
|
marker_pub.publish(l_joints);
|
||||||
|
marker_pub.publish(r_joints);
|
||||||
marker_pub.publish(l_arm);
|
marker_pub.publish(l_arm);
|
||||||
marker_pub.publish(r_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(camera);
|
||||||
i = i +1;
|
marker_pub.publish(base);
|
||||||
|
|
||||||
r.sleep();
|
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