From 8861d215c5bce16905677ed0ff6d7cf849cc8d6d Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Fri, 8 Feb 2019 16:42:48 +0100 Subject: [PATCH] refactored the code, fixed config bug --- config/default.yaml | 2 +- launch/calibration.launch | 12 + launch/fulltest.launch | 10 + launch/rviz_config/human.rviz | 369 ++------------ launch/rviz_config/nao.rviz | 727 +++++++++++++++------------ script/controller.py | 6 +- script/fall_detector.py | 9 +- script/imitator.py | 33 +- script/masterloop.py | 21 +- script/walker.py | 6 +- src/rviz_human.cpp | 891 +++++++++++++++++++--------------- 11 files changed, 1022 insertions(+), 1064 deletions(-) create mode 100644 launch/calibration.launch 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; -} - -