diff --git a/config/default.yaml b/config/default.yaml
index aea6fb4..496ca25 100644
--- a/config/default.yaml
+++ b/config/default.yaml
@@ -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]
diff --git a/launch/calibration.launch b/launch/calibration.launch
new file mode 100644
index 0000000..c786f91
--- /dev/null
+++ b/launch/calibration.launch
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/fulltest.launch b/launch/fulltest.launch
index e65b32d..7770599 100644
--- a/launch/fulltest.launch
+++ b/launch/fulltest.launch
@@ -15,4 +15,14 @@
output="screen"/>
+
+
+
+
+
+
+
+
diff --git a/launch/rviz_config/human.rviz b/launch/rviz_config/human.rviz
index b39c597..3f8ba9f 100644
--- a/launch/rviz_config/human.rviz
+++ b/launch/rviz_config/human.rviz
@@ -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:
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:
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
diff --git a/launch/rviz_config/nao.rviz b/launch/rviz_config/nao.rviz
index 585da34..40c29e9 100644
--- a/launch/rviz_config/nao.rviz
+++ b/launch/rviz_config/nao.rviz
@@ -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:
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
diff --git a/script/controller.py b/script/controller.py
index 232fc54..302e233 100755
--- a/script/controller.py
+++ b/script/controller.py
@@ -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
diff --git a/script/fall_detector.py b/script/fall_detector.py
index 58de4cc..0cc3f3c 100755
--- a/script/fall_detector.py
+++ b/script/fall_detector.py
@@ -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
diff --git a/script/imitator.py b/script/imitator.py
index 913a1a7..fce11aa 100755
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -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)
diff --git a/script/masterloop.py b/script/masterloop.py
index 7a7ab93..bb336bf 100755
--- a/script/masterloop.py
+++ b/script/masterloop.py
@@ -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'):
diff --git a/script/walker.py b/script/walker.py
index 9c54735..fbd3358 100755
--- a/script/walker.py
+++ b/script/walker.py
@@ -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():
diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp
index 2224163..3a0e219 100644
--- a/src/rviz_human.cpp
+++ b/src/rviz_human.cpp
@@ -4,401 +4,534 @@
#include
#include
-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_marker", 10
- );
+ ros::init(argc, argv, "rviz_human");
+ ros::NodeHandle n;
+ ros::Rate r(100);
+ ros::Publisher marker_pub = n.advertise(
+ "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;
-}
-
-