diff --git a/hrpsys_ros_bridge_jvrc/CMakeLists.txt b/hrpsys_ros_bridge_jvrc/CMakeLists.txt index 43e5889d..2ee26475 100644 --- a/hrpsys_ros_bridge_jvrc/CMakeLists.txt +++ b/hrpsys_ros_bridge_jvrc/CMakeLists.txt @@ -11,6 +11,11 @@ pkg_check_modules(hrpsys hrpsys-base REQUIRED) ##catkin_python_setup() +add_message_files( + DIRECTORY msg + FILES MultiGaitGoPos.msg MultiGaitGoPosArray.msg + ) + add_service_files( DIRECTORY srv FILES StringRequest.srv diff --git a/hrpsys_ros_bridge_jvrc/config/jaxon_jvrc_self_filter.yaml b/hrpsys_ros_bridge_jvrc/config/jaxon_jvrc_self_filter.yaml new file mode 100644 index 00000000..00796b78 --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/config/jaxon_jvrc_self_filter.yaml @@ -0,0 +1,80 @@ +min_sensor_dist: .2 +self_see_default_padding: .01 +self_see_default_scale: 1.0 +self_see_links: + - name: BODY + padding: .05 + - name: RLEG_LINK0 + padding: .05 + - name: RLEG_LINK1 + padding: .05 + - name: RLEG_LINK2 + padding: .05 + - name: RLEG_LINK3 + padding: .05 + - name: RLEG_LINK4 + padding: .05 + - name: RLEG_LINK5 + padding: .05 + - name: LLEG_LINK0 + padding: .05 + - name: LLEG_LINK1 + padding: .05 + - name: LLEG_LINK2 + padding: .05 + - name: LLEG_LINK3 + padding: .05 + - name: LLEG_LINK4 + padding: .05 + - name: LLEG_LINK5 + padding: .05 + - name: CHEST_LINK0 + padding: .05 + - name: CHEST_LINK1 + padding: .05 + - name: CHEST_LINK2 + padding: .05 + - name: HEAD_LINK0 + padding: .05 + - name: HEAD_LINK1 + padding: .05 + - name: RARM_LINK0 + padding: .05 + - name: RARM_LINK1 + padding: .05 + - name: RARM_LINK2 + padding: .05 + - name: RARM_LINK3 + padding: .05 + - name: RARM_LINK4 + padding: .05 + - name: RARM_LINK5 + padding: .05 + - name: RARM_LINK6 + padding: .05 + - name: RARM_LINK7 + padding: .05 + - name: LARM_LINK0 + padding: .05 + - name: LARM_LINK1 + padding: .05 + - name: LARM_LINK2 + padding: .05 + - name: LARM_LINK3 + padding: .05 + - name: LARM_LINK4 + padding: .05 + - name: LARM_LINK5 + padding: .05 + - name: LARM_LINK6 + padding: .05 + - name: LARM_LINK7 + padding: .05 + - name: RARM_FINGER0 + padding: .15 + - name: RARM_FINGER1 + padding: .15 + - name: LARM_FINGER0 + padding: .15 + - name: LARM_FINGER1 + padding: .15 diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index 3dfdfd6d..acd5de17 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -3,9 +3,16 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.570997 - Tree Height: 463 + Expanded: + - /Grid1 + - /PointCloudGroup1 + - /PointCloudGroup1/stereo1 + - /PointCloudGroup1/laser1 + - /select-contact-plane1 + - /select-contact-plane1/PolygonArray1 + - /select-contact-plane1/QuietInteractiveMarker1 + Splitter Ratio: 0.413567 + Tree Height: 1107 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -24,12 +31,12 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: LaserScan Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 1 + Cell Size: 0.5 Class: rviz/Grid Color: 160; 160; 164 Enabled: true @@ -43,9 +50,87 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 100 Reference Frame: Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 3209 + Min Color: 0; 0; 0 + Min Intensity: 44 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /multisense/lidar_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /jsk_model_marker_interface/update + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: false + Show Visual Aids: true + Update Topic: /urdf_control_marker/update + Value: true + Enabled: true + Name: out-of-body-marker + - Class: rviz/Group + Displays: + - Class: jsk_rviz_plugin/OverlayMenu + Enabled: true + Name: OverlayMenu + Topic: /rviz_menu_text + Value: true + - Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 25; 255; 240 + Name: OverlayText + Overtake Color Properties: false + Overtake Position Properties: false + Topic: /gait_state_text + Value: true + font: DejaVu Sans Mono + height: 128 + left: 0 + line width: 2 + text size: 12 + top: 0 + width: 128 + Enabled: true + Name: message - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -85,16 +170,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - LARM_FINGER0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - LARM_FINGER1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true LARM_LINK0: Alpha: 1 Show Axes: false @@ -165,22 +240,22 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Link Tree Style: Links in Alphabetic Order - RANGE_LINK: + L_thk_finger1: Alpha: 1 Show Axes: false Show Trail: false Value: true - RARM_FINGER0: + L_thk_finger2: Alpha: 1 Show Axes: false Show Trail: false Value: true - RARM_FINGER1: + L_thk_palm: Alpha: 1 Show Axes: false Show Trail: false Value: true + Link Tree Style: Links in Alphabetic Order RARM_LINK0: Alpha: 1 Show Axes: false @@ -251,143 +326,249 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + R_thk_finger1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_thk_finger2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_thk_palm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + accel: + Alpha: 1 + Show Axes: false + Show Trail: false + center_bottom_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + center_top_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + fisheye: + Alpha: 1 + Show Axes: false + Show Trail: false + gyro: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true head_end_coords: Alpha: 1 Show Axes: false Show Trail: false + head_hokuyo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_root: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true larm_end_coords: Alpha: 1 Show Axes: false Show Trail: false + left_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + left_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + left_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + lfsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + lhsensor: + Alpha: 1 + Show Axes: false + Show Trail: false lleg_end_coords: Alpha: 1 Show Axes: false Show Trail: false + mag: + Alpha: 1 + Show Axes: false + Show Trail: false + motor: + Alpha: 1 + Show Axes: false + Show Trail: false rarm_end_coords: Alpha: 1 Show Axes: false Show Trail: false + rfsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + rhsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + right_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + right_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + right_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false rleg_end_coords: Alpha: 1 Show Axes: false Show Trail: false + spindle: + Alpha: 1 + Show Axes: false + Show Trail: false Name: RobotModel Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 + - Alpha: 0.5 + Class: jsk_rviz_plugin/Footstep Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Flat Squares - Topic: /multisense/organized_image_points2_color - Use Fixed Frame: true - Use rainbow: true + Name: Footstep + Show Name: true + Topic: /go_pos_footsteps + Use Group Coloring: false Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + Width: 0.15 + depth: 0.3 + height: 0.01 + - Class: jsk_rviz_plugin/BoundingBoxArray Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 1000 - Min Color: 0; 0; 0 - Min Intensity: 1000 - Name: LaserScan - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Flat Squares - Topic: /multisense/lidar_scan - Use Fixed Frame: true - Use rainbow: true + Name: BoundingBoxArray + Topic: /go_pos_footsteps_boundingbox Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Auto + line width: 0.005 + only edge: true + show coords: false - Class: rviz/Group Displays: - - Class: rviz/InteractiveMarkers - Enable Transparency: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /jsk_model_marker_interface/update - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: stereo + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.002 + Style: Flat Squares + Topic: /multisense_local/organized_image_points2_color + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.149959 + Min Value: -0.041234 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /urdf_control_marker/update + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.005 + Style: Flat Squares + Topic: /gait_planner_group/laser_z_filter/output + Use Fixed Frame: true + Use rainbow: true Value: true Enabled: true - Name: out-of-body-marker + Name: PointCloudGroup - Class: rviz/Group Displays: - - Class: jsk_rviz_plugin/OverlayMenu + - Alpha: 1 + Class: jsk_rviz_plugin/PolygonArray + Color: 25; 255; 0 Enabled: true - Name: OverlayMenu - Topic: /rviz_menu_text - Value: true - - Background Alpha: 0.8 - Background Color: 0; 0; 0 - Class: jsk_rviz_plugin/OverlayText + Name: PolygonArray + Topic: /polygon_select_group/region_growing_multiple_plane_segmentation/output/polygons_throttle + Value: true + coloring: Auto + normal length: 0.1 + only border: true + show normal: true + - Class: jsk_rviz_plugin/QuietInteractiveMarker + Enable Transparency: true Enabled: true - Foreground Alpha: 0.8 - Foreground Color: 25; 255; 240 - Name: OverlayText - Overtake Color Properties: false - Overtake Position Properties: false - Topic: /gait_state_text + Name: QuietInteractiveMarker + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /polygon_select_group/polygon_interactive_marker/update Value: true - font: DejaVu Sans Mono - height: 128 - left: 0 - line width: 2 - text size: 12 - top: 0 - width: 128 Enabled: true - Name: message + Name: select-contact-plane Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: odom_on_ground + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -404,34 +585,35 @@ Visualization Manager: - Class: rviz/PublishPoint Single click: true Topic: /clicked_point + - Class: jsk_rviz_plugin/CloseAll Value: true Views: Current: Class: rviz/Orbit - Distance: 5.89461 + Distance: 4.3294 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.191248 - Y: -0.811557 - Z: 0.790978 + X: 1.44266 + Y: 0.449749 + Z: 0.543117 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.645398 + Pitch: 0.619797 Target Frame: Value: Orbit (rviz) - Yaw: 2.6754 + Yaw: 1.46173 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 744 + Height: 1388 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002a80000025efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000025e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005560000003efc0100000002fb0000000800540069006d0065010000000000000556000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005560000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a7000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000004e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d0065010000000000000500000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000500000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -440,6 +622,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1366 - X: 0 + Width: 1280 + X: -7 Y: 24 diff --git a/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l b/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l index b5f099f2..15acd2bb 100644 --- a/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l +++ b/hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l @@ -9,7 +9,6 @@ (:check-ready () (speak-en "checking if i am ready" :timeout 1) - (send *ri* :stop-grasp) (send *ri* :start-grasp) (let ((abc-mode (send *ri* :get-auto-balancer-controller-mode)) (st-mode (send *ri* :get-st-controller-mode)) @@ -317,7 +316,7 @@ :gait-generator-param (list :default-step-time (* speed-up-ratio biped-step-time) ;speed up :default-double-support-ratio (let ((swing-time (* (- 1.0 biped-double-support-ratio) biped-step-time))) (- 1.0 (/ swing-time (* speed-up-ratio biped-step-time)))) - :default-step-height 0.05 + :default-step-height 0.08 :default-orbit-type 2 ;RECTANGLE :stride-parameter (float-vector 0.15 0.10 20.0 0.15) :leg-default-translate-pos (list (send (send *robot* :rleg :end-coords) :worldpos) @@ -375,9 +374,9 @@ (send *ri* :set-st-param :emergency-check-mode 0) (progn ;; RYOBI HOIST - ;; (send *ri* :angle-vector touch-down-pose 5000) + (send *ri* :angle-vector touch-down-pose 3000) ;; IREX HOIST - (send *ri* :angle-vector touch-down-pose 6000) + ;; (send *ri* :angle-vector touch-down-pose 6000) (speak-en "u go e te ru" :timeout 1) (send *ri* :wait-interpolation)) (progn diff --git a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l index fdec6ebc..a06cde50 100755 --- a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l +++ b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l @@ -12,7 +12,9 @@ :slots (robot-name tfl reference-frame-id use-robot-interface-flag potentio-vector joint-name-list - mg) + mg + selected-polygon-relative-to-body + tripedal-av) ) (defmethod robot-menu @@ -22,14 +24,17 @@ (setq robot-name (string-upcase (if (unix::getenv "ROBOT") (unix::getenv "ROBOT") "jaxon_jvrc")) tfl (instance ros::transform-listener :init) reference-frame-id (ros::get-param "~reference_frame_id" "/ground") - use-robot-interface-flag (ros::get-param "~use_robot_interface" t)) + use-robot-interface-flag (ros::get-param "~use_robot_interface" t) + selected-polygon-relative-to-body nil) ;; generate *robot* (when (or (not (boundp '*robot*)) (not *robot*)) (load (robot-file robot-name)) (setq *robot* (if use-robot-interface-flag (init-robot-from-name robot-name) - (make-robot-model-from-name robot-name)))) - (setq mg (instance multi-gait :init :robot robot-name :make-robot-model nil)) + (make-robot-model-from-name robot-name))) + (objects (list *robot*))) + (setq mg (instance multi-gait :init :robot robot-name :make-robot-model nil) + tripedal-av (send *robot* :reset-pose)) ;; subscriber (ros::subscribe "/joint_states" sensor_msgs::JointState #'send self :joint-states-callback) (ros::subscribe "/send_angle_command" std_msgs::empty #'send self :send-angle-to-real-robot-one-cb) @@ -37,10 +42,40 @@ (ros::subscribe "/set_robot_pose" std_msgs::empty #'send self :set-robot-pose-cb) (ros::subscribe "/set_robot_pose_with_av" std_msgs::empty #'send self :set-robot-pose-with-av-cb) (ros::subscribe "/gait_menu_command" std_msgs::empty #'send self :gait-menu-command-cb) + (ros::subscribe "/polygon_select_group/polygon_marker/selected_polygon" geometry_msgs::PolygonStamped #'send self :polygon-stamped-cb) ;; publisher (ros::advertise "/jsk_model_marker_interface/robot/reset_joint_states" sensor_msgs::JointState 1) (ros::advertise "/urdf_control_marker/set_pose" geometry_msgs::PoseStamped 1) (warning-message 2 (format nil "[~A] initialize robot-menu.~%" (ros::get-name))) + ;; server + (ros::advertise-service "/move_quadruped" hrpsys_ros_bridge_jvrc::StringRequest #'send self :move-quadruped) + (ros::advertise-service "/move_biped" hrpsys_ros_bridge_jvrc::StringRequest #'send self :move-biped) + ) + (:move-quadruped + (req) + (let (fsm-res res) + (setq res (send req :response)) + (setq fsm-res (ros::service-call "/call_gait_state_event" (instance hrpsys_ros_bridge_jvrc::StringRequestRequest :init :data "to quadruped"))) + (if (send fsm-res :result) + (progn + (send mg :biped->quadruped) + (ros::service-call "/call_gait_state_event" (instance hrpsys_ros_bridge_jvrc::StringRequestRequest :init :data "finish quadruped")) + (send res :result t)) + (send res :result nil)) + res) + ) + (:move-biped + (req) + (let (fsm-res res) + (setq res (send req :response)) + (setq fsm-res (ros::service-call "/call_gait_state_event" (instance hrpsys_ros_bridge_jvrc::StringRequestRequest :init :data "to biped"))) + (if (send fsm-res :result) + (progn + (send mg :quadruped->biped) + (ros::service-call "/call_gait_state_event" (instance hrpsys_ros_bridge_jvrc::StringRequestRequest :init :data "finish biped")) + (send res :result t)) + (send res :result nil)) + res) ) (:joint-states-callback (msg) @@ -106,6 +141,7 @@ ":reset-pose" ":reset-manip-pose" ":init-pose" + ":tripedal-pose" ":quadruped-pose")) (req (instance drc_task_common::RvizMenuCallRequest :init @@ -120,6 +156,10 @@ (send *robot* :angle-vector potentio-vector))) ((find-method *robot* (read-from-string res-value)) (eval (list 'send '*robot* '(read-from-string res-value)))) + ((equal res-value ":tripedal-pose") + (if selected-polygon-relative-to-body + (send *robot* :angle-vector tripedal-av) + )) (t (send *robot* :angle-vector (mg . quadruped-pose))) ) @@ -176,6 +216,92 @@ (instance hrpsys_ros_bridge_jvrc::StringRequestRequest :init :data (if (eq gt 'biped) "biped stop" "quadruped stop"))))))) )) )) + (:polygon-stamped-cb + (msg) + (send tfl :wait-for-transform "/BODY" (send msg :header :frame_id) (send msg :header :stamp) 1) + (let* ((target-relative-to-body + (send tfl :lookup-transform "/BODY" + (send msg :header :frame_id) + (send msg :header :stamp))) + (ros-points (send msg :polygon :points)) + ;; vertices are relative to BODY + (vertices (mapcar #'(lambda (p) + (send target-relative-to-body :transform-vector (ros::tf-point->pos p))) + ros-points))) + (setq selected-polygon-relative-to-body (instance polygon :init :vertices vertices)) + (warning-message 3 (format nil "[~A] selected polygon is stored.~%" (ros::get-name))) + (progn ;for debug + (if use-robot-interface-flag + (send *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send *robot* :angle-vector potentio-vector)) + (send *robot* :fix-leg-to-coords (make-coords))) + ;; generate target coords relative to current world frame + (let (target-coords use-arm) + (let* ((ez (scale -1 (send selected-polygon-relative-to-body :normal))) + (ex (v* (float-vector 0 1 0) ez)) + (ey (v* ez ex)) + (selected-polygon-pos-relative-to-world + (send (send (car (send *robot* :links)) :copy-worldcoords) + :transform-vector (cadr (send selected-polygon-relative-to-body :centroid))))) + (setq target-coords (make-coords :pos selected-polygon-pos-relative-to-world + :rot (m* (send (send (car (send *robot* :links)) :copy-worldcoords) :worldrot) + (transpose (matrix (normalize-vector ex) + (normalize-vector ey) + (normalize-vector ez))))))) + ;; use nearest arm + (setq use-arm + (if (> (elt (cadr (send selected-polygon-relative-to-body :centroid)) 1) 0) + (list :larm :lhand-contact-coords) + (list :rarm :rhand-contact-coords))) + ;; solve ik with search + (let (new-tc (x-times 10) (y-times 10) + (search-x 500) (search-y 500) + ik-success best-pair (best-manip 0)) + (dotimes (i x-times) + (dotimes (j y-times) + (setq new-tc (send (send target-coords :copy-worldcoords) :translate (float-vector (* search-x (/ (- i (/ x-times 2.0)) x-times)) + (* search-y (/ (- j (/ y-times 2.0)) y-times)) + 0) :local)) + (when (eq ':inside + (send selected-polygon-relative-to-body :insidep + (send (send (send (send (car (send *robot* :links)) :copy-worldcoords) :inverse-transformation) :transform (send new-tc :copy-worldcoords)) :worldpos))) + ;; solve ik + (setq ik-success (send *robot* :inverse-kinematics + (send new-tc :translate #f(0 0 100) :local) + :move-target (send *robot* :get (elt use-arm 1)) + :link-list (send *robot* :link-list (send (send *robot* :get (elt use-arm 1)) :parent) (send *robot* (elt use-arm 0) :root-link)) + :rotation-axis :z + :translation-axis t + :debug-view nil + :dump-command nil)) + (if ik-success + (let ((cur-manip (manipulability (send *robot* :calc-jacobian-from-link-list + (send *robot* :link-list (send (send *robot* (elt use-arm 0) :end-coords) :parent)) + :move-target (send *robot* (elt use-arm 0) :end-coords) + :translation-axis '(t) + :rotation-axis '(t))))) + (when (> cur-manip best-manip) + (setq best-manip cur-manip) + (setq best-pair (list i j)))))) + )) + (if best-pair + (progn + (setq new-tc (send (send target-coords :copy-worldcoords) :translate (float-vector (* search-x (/ (- (elt best-pair 0) (/ x-times 2.0)) x-times)) + (* search-y (/ (- (elt best-pair 1) (/ y-times 2.0)) y-times)) + 0) :local)) + (send *robot* :inverse-kinematics + (send new-tc :translate #f(0 0 100) :local) + :move-target (send *robot* :get (elt use-arm 1)) + :link-list (send *robot* :link-list (send (send *robot* :get (elt use-arm 1)) :parent) (send *robot* (elt use-arm 0) :root-link)) + :rotation-axis :z + :translation-axis t + :debug-view :no-message + :dump-command nil) + (setq tripedal-av (send *robot* :angle-vector))) + (warning-message 2 (format nil "[~A] failed to solve ik to the polygon.~%" (ros::get-name)))) + ) + ) + )) ) ;; main @@ -189,6 +315,7 @@ (ros::wait-for-service s))) (instance robot-menu :init) (while t + (x::window-main-one) (ros::sleep) (ros::spin-once) ) diff --git a/hrpsys_ros_bridge_jvrc/launch/heightmap.launch b/hrpsys_ros_bridge_jvrc/launch/heightmap.launch new file mode 100644 index 00000000..a3ec0f20 --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/launch/heightmap.launch @@ -0,0 +1,123 @@ + + + + + + + + + nodelets: + - name: robot_center_pointcloud + type: jsk_pcl/TfTransformCloud + remappings: + - from: ~input + to: $(arg INPUT) + - name: laser_x_filter + type: pcl/PassThrough + remappings: + - from: ~input + to: robot_center_pointcloud/output + - from: /tf + to: /tf_null + - name: laser_y_filter + type: pcl/PassThrough + remappings: + - from: ~input + to: laser_x_filter/output + - from: /tf + to: /tf_null + - name: laser_z_filter + type: pcl/PassThrough + remappings: + - from: ~input + to: laser_y_filter/output + - from: /tf + to: /tf_null + - name: latest_heightmap + type: jsk_pcl/HeightmapConverter + remappings: + - from: ~input + to: laser_z_filter/output + - name: latest_complete_heightmap + type: jsk_pcl/HeightmapMorphologicalFiltering + remappings: + - from: ~input + to: latest_heightmap/output + - name: accumulated_heightmap + type: jsk_pcl/HeightmapTimeAccumulation + remappings: + - from: ~input + to: latest_complete_heightmap/output + - from: ~input/prev_pointcloud + to: accumulated_heightmap_pointcloud/output + - name: accumulated_heightmap_pointcloud + type: jsk_pcl/HeightmapToPointCloud + remappings: + - from: ~input + to: accumulated_heightmap/output + - name: accumulated_heightmap_pointcloud_static + type: jsk_pcl/TfTransformCloud + remappings: + - from: ~input + to: accumulated_heightmap_pointcloud/output + + + + target_frame_id: $(arg ROBOT_FRAME) + + + + filter_field_name: x + filter_limit_min: 0.3 + filter_limit_max: 2.8 + + + + + filter_field_name: y + filter_limit_min: -1.0 + filter_limit_max: 1.0 + + + + + filter_field_name: z + filter_limit_min: -0.05 + filter_limit_max: 0.15 + + + + min_x: 0.3 + max_x: 2.8 + min_y: -1.0 + max_y: 1.0 + resolution_x: 1000 + resolution_y: 800 + + + center_frame_id: $(arg ROBOT_FRAME) + fixed_frame_id: $(arg STATIC_FRAME) + + + target_frame_id: $(arg STATIC_FRAME) + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_ros_bridge_jvrc/launch/jaxon_jvrc_pointcloud_minimal.launch b/hrpsys_ros_bridge_jvrc/launch/jaxon_jvrc_pointcloud_minimal.launch new file mode 100644 index 00000000..3ff64def --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/launch/jaxon_jvrc_pointcloud_minimal.launch @@ -0,0 +1,66 @@ + + + + + + scan_filter_chain: + - name: shadows + type: laser_filters/ScanShadowsFilter + params: + min_angle: 0 + max_angle: 165 + neighbors: 5 + window: 5 + - name: dark_shadows + type: LaserScanIntensityFilter + params: + lower_threshold: 200 + upper_threshold: 10000 + disp_histogram: 0 + - name: range + type: laser_filters/LaserScanRangeFilter + params: + lower_threshold: 0.2 # 0.5 + upper_threshold: 10 + high_fidelity: true + target_frame: odom + + + + + + + + + + + + nodelets: + - name: tilt_laser_listener + type: jsk_pcl/TiltLaserListener + remappings: + - from: ~input + to: /joint_states + - from: ~input/cloud + to: /multisense/cloud_self_filtered + - name: multisense_laser_relay + type: jsk_topic_tools/Relay + remappings: + - from: ~input + to: tilt_laser_listener/output_cloud + - from: ~output + to: /full_cloud2 + + + + + laser_type: infinite_spindle + joint_name: motor_joint + use_laser_assembler: true + not_use_laser_assembler_service: true + twist_frame_id: head_hokuyo_frame + overwrap_angle: 0.0 + skip_number: 10 + + + diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index ae7094f2..ee6a65e2 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -3,7 +3,7 @@ - + @@ -25,6 +25,8 @@ + + @@ -41,6 +43,10 @@ + + + + diff --git a/hrpsys_ros_bridge_jvrc/launch/polygon_select.launch b/hrpsys_ros_bridge_jvrc/launch/polygon_select.launch new file mode 100644 index 00000000..3724c31d --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/launch/polygon_select.launch @@ -0,0 +1,75 @@ + + + + + nodelets: + - name: laser_x_filter + type: pcl/PassThrough + remappings: + - from: ~input + to: /full_cloud2 + - from: /tf + to: /tf_null + - name: laser_y_filter + type: pcl/PassThrough + remappings: + - from: ~input + to: laser_x_filter/output + - from: /tf + to: /tf_null + - name: laser_z_filter + type: pcl/PassThrough + remappings: + - from: ~input + to: laser_y_filter/output + - from: /tf + to: /tf_null + + + + + filter_field_name: x + filter_limit_min: -5.0 + filter_limit_max: 1000.0 + + + + + filter_field_name: y + filter_limit_min: -1.0 + filter_limit_max: 1.0 + + + + + filter_field_name: z + filter_limit_min: 0.8 + filter_limit_max: 2.0 + + + + + + + + k_search: 50 + + + + + + + + max_area: 1.0 + min_area: 0.01 + + + + + + + + + + diff --git a/hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPos.msg b/hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPos.msg new file mode 100644 index 00000000..76fdadc5 --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPos.msg @@ -0,0 +1,11 @@ +float64 x +float64 y +float64 th + +int64 BIPED=0 +int64 TROT=1 +int64 PACE=2 +int64 CRAWL=3 +int64 GALLOP=4 + +int64 gait_type diff --git a/hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPosArray.msg b/hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPosArray.msg new file mode 100644 index 00000000..b2a21877 --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPosArray.msg @@ -0,0 +1,2 @@ +Header header +MultiGaitGoPos[] orders diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py new file mode 100755 index 00000000..d35d05a8 --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python + +import rospy, tf, cv_bridge +import cv2, numpy +import std_msgs.msg, jsk_footstep_msgs.msg, visualization_msgs.msg, geometry_msgs.msg, jsk_recognition_msgs.msg, sensor_msgs.msg, hrpsys_ros_bridge_jvrc.msg +import hrpsys_ros_bridge.srv, drc_task_common.srv, hrpsys_ros_bridge_jvrc.srv + +class GaitPlannerClass(object): + def __init__(self, node_name='gait_planner_node'): + rospy.init_node(node_name) + # tf + self.tfl = tf.TransformListener() + # store heightmap + self.bridge = cv_bridge.CvBridge() + self.my_heightmap_header = None + self.my_heightmap = None + # store contact plane candidate + self.contact_plane_candidate = None + # Publisher + self.go_pos_footsteps_bb_pub = rospy.Publisher('/go_pos_footsteps_boundingbox', jsk_recognition_msgs.msg.BoundingBoxArray, queue_size=1) + self.masked_heightmap_pub = rospy.Publisher('/masked_heightmap', sensor_msgs.msg.Image, queue_size=1) + self.multi_go_pos_order_pub = rospy.Publisher('/multi_go_pos_order', hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPosArray, queue_size=1) + # subscriber + rospy.Subscriber('/go_pos_footsteps', jsk_footstep_msgs.msg.FootstepArray, self.footstep_callback) + rospy.Subscriber('~input/heightmap', sensor_msgs.msg.Image, self.heightmap_callback) + ### https://github.com/jsk-ros-pkg/jsk_visualization/pull/539 + rospy.Subscriber('~input/contact_plane_candidate', geometry_msgs.msg.PolygonStamped, self.contact_plane_candidate_callback) + self.fsm_call_srv = rospy.ServiceProxy('/call_gait_state_event', hrpsys_ros_bridge_jvrc.srv.StringRequest) + rospy.wait_for_service("/StabilizerServiceROSBridge/getParameter") + self.st_param = rospy.ServiceProxy("/StabilizerServiceROSBridge/getParameter", hrpsys_ros_bridge.srv.OpenHRP_StabilizerService_getParameter)() + rospy.wait_for_service("/AutoBalancerServiceROSBridge/getGaitGeneratorParam") + self.get_gg_param = rospy.ServiceProxy("/AutoBalancerServiceROSBridge/getGaitGeneratorParam", hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_getGaitGeneratorParam) + self.gg_param = None + self.max_keep_gait_counter = 3 + + def footstep_callback(self, msg): + bb_msg = self._jsk_footstep_msgs_to_bounding_box_array(msg) + self.go_pos_footsteps_bb_pub.publish(bb_msg) + if self.my_heightmap == None: + rospy.logwarn("[%s] my_heightmap : %s", rospy.get_name(), (self.my_heightmap != None)) + return + else: + transformed_bb_msg = self._tf_transform_bounding_box_array(bb_msg) + vertices_list = self._vertices_of_bounding_box_array(transformed_bb_msg) + x_min = rospy.get_param("/gait_planner_group/latest_heightmap/min_x") + x_max = rospy.get_param("/gait_planner_group/latest_heightmap/max_x") + y_min = rospy.get_param("/gait_planner_group/latest_heightmap/min_y") + y_max = rospy.get_param("/gait_planner_group/latest_heightmap/max_y") + x_pix = rospy.get_param("/gait_planner_group/latest_heightmap/resolution_x") + y_pix = rospy.get_param("/gait_planner_group/latest_heightmap/resolution_y") + vertices_list_in_pixel = self._meter_to_pixel(vertices_list, x_min, x_max, y_min, y_max, x_pix, y_pix) + # generate mask image + thre, occlusion_mask_image = cv2.threshold(self.my_heightmap, -1e+3, 255, cv2.THRESH_BINARY) + occlusion_mask_image = numpy.uint8(occlusion_mask_image) + # calc MultiGoPos order + self.gg_param = self.get_gg_param() + origin = self._calc_reference_pose(msg.footsteps[0]) + prev_origin = self._calc_reference_pose(msg.footsteps[0]) + gait_type = hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.BIPED + mgg_array = hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPosArray(header=msg.header) + keep_gait_counter = self.max_keep_gait_counter + for idx, (vs, fs) in enumerate(zip(vertices_list_in_pixel, msg.footsteps)): + footstep_mask_image = numpy.zeros((y_pix, x_pix, 1), numpy.uint8) + cv2.fillConvexPoly(footstep_mask_image, numpy.int32(numpy.array([vs[0], vs[1], vs[2], vs[3]])), 255) + mask_image = cv2.bitwise_and(occlusion_mask_image, occlusion_mask_image, mask=footstep_mask_image) + rospy.logwarn("%s : %s", idx, fs.leg) + rospy.logwarn("minMaxLoc : %s", cv2.minMaxLoc(self.my_heightmap, mask=mask_image)) + rospy.logwarn("meanStdDev : %s", cv2.meanStdDev(self.my_heightmap, mask=mask_image)) + mean, stddev = cv2.meanStdDev(self.my_heightmap, mask=mask_image) + if stddev[0,0] > 0.01: + if gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.BIPED: + mgg_msg = self._get_multi_gait_go_pos(origin, prev_origin) + mgg_msg.gait_type = gait_type + mgg_array.orders.append(mgg_msg) + origin = prev_origin + gait_type = hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.TROT + keep_gait_counter = self.max_keep_gait_counter + elif stddev[0,0] < 0.008: + if gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.TROT: + if keep_gait_counter > 0: + keep_gait_counter -= 1 + else: + new_origin = self._calc_reference_pose(fs) + mgg_msg = self._get_multi_gait_go_pos(origin, new_origin) + mgg_msg.gait_type = gait_type + mgg_array.orders.append(mgg_msg) + origin = new_origin + gait_type = hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.BIPED + else: + if keep_gait_counter > 0: + keep_gait_counter -= 1 + rospy.logwarn("keep_gait_counter : %s, gait_type : %s", keep_gait_counter, gait_type) + # else: # keep + # if keep_gait_counter > 0: + # keep_gait_counter -= 1 + if idx == len(msg.footsteps) - 1: # final step + fin_origin = self._calc_reference_pose(fs) + mgg_msg = self._get_multi_gait_go_pos(origin, fin_origin) + mgg_msg.gait_type = gait_type + mgg_array.orders.append(mgg_msg) + prev_origin = self._calc_reference_pose(fs) # keep the previous foot step + self.multi_go_pos_order_pub.publish(mgg_array) + + def heightmap_callback(self, msg): + self.my_heightmap_header = msg.header + self.my_heightmap = self.bridge.imgmsg_to_cv2(msg) # second argument should be not specified + + def contact_plane_candidate_callback(self, msg): + self.contact_plane_candidate = msg + + def _jsk_footstep_msgs_to_bounding_box_array(self, fsl): + box_array = jsk_recognition_msgs.msg.BoundingBoxArray() + box_array.header = fsl.header + for fs in fsl.footsteps: + box = jsk_recognition_msgs.msg.BoundingBox() + box.header = fsl.header + box.label = int(fs.cost) # order for debug + box.pose = fs.pose + box.dimensions = fs.dimensions + # expand region + box.dimensions.x += 0.2 + box.dimensions.y += 0.2 + box.dimensions.z += 0.5 + # fix position because bounding box origin is the center of bounding box and x / y /z is edge length + i_param = self.st_param.i_param + box.pose.position.x += (i_param.eefm_leg_front_margin - i_param.eefm_leg_rear_margin) / 2.0 + box.pose.position.y += (i_param.eefm_leg_inside_margin - i_param.eefm_leg_outside_margin) / 2.0 + box_array.boxes.append(box) + return box_array + + + def _tf_transform_bounding_box_array(self, bb_array): + if self.tfl.frameExists(bb_array.header.frame_id) and self.tfl.frameExists(self.my_heightmap_header.frame_id): + for box in bb_array.boxes: + tmp_pose_stamped = geometry_msgs.msg.PoseStamped(header=box.header, pose=box.pose) + new_pose_stamped = self.tfl.transformPose(self.my_heightmap_header.frame_id, tmp_pose_stamped) + box.pose = new_pose_stamped.pose + return bb_array + else: + rospy.logwarn("[%s] failed to solve tf from %s (%s) to %s (%s)", rospy.get_name(), bb_array.header.frame_id, self.tfl.frameExists(bb_array.header.frame_id), self.my_heightmap_header, self.tfl.frameExists(self.my_heightmap_header.frame_id)) + return + + def _vertices_of_bounding_box_array(self, bb_array): + ret = [] + for box in bb_array.boxes: + position = box.pose.position + orientation = box.pose.orientation + dimensions = box.dimensions + mat = tf.transformations.quaternion_matrix([orientation.x, orientation.y, orientation.z, orientation.w]) + directions = [numpy.array([+1*dimensions.x/2, +1*dimensions.y/2, 0, 0]), + numpy.array([+1*dimensions.x/2, -1*dimensions.y/2, 0, 0]), + numpy.array([-1*dimensions.x/2, -1*dimensions.y/2, 0, 0]), + numpy.array([-1*dimensions.x/2, +1*dimensions.y/2, 0, 0])] + vertices = [numpy.dot(mat, d)[:2] + numpy.array([position.x, position.y]) for d in directions] + ret.append(vertices) + # rospy.logwarn("%s, %s, %s, %s", + # [int(x) for x in ret[0][0]*1e3], + # [int(x) for x in ret[0][1]*1e3], + # [int(x) for x in ret[0][2]*1e3], + # [int(x) for x in ret[0][3]*1e3]) + return ret + + def _meter_to_pixel(self, vertices_list, x_min, x_max, y_min, y_max, x_pix, y_pix): + for vs in vertices_list: + for v in vs: + v[0] = int((v[0] - x_min) / (x_max - x_min) * x_pix) + v[1] = int((v[1] - y_min) / (y_max - y_min) * y_pix) + return vertices_list + + def _calc_reference_pose(self, fs): + ret = geometry_msgs.msg.Pose() + position = fs.pose.position + orientation = fs.pose.orientation + leg = fs.leg + if leg == jsk_footstep_msgs.msg.Footstep.RLEG: + offset = self.gg_param.i_param.leg_default_translate_pos.data[0*3+1] + elif leg == jsk_footstep_msgs.msg.Footstep.LLEG: + offset = self.gg_param.i_param.leg_default_translate_pos.data[1*3+1] + elif leg == jsk_footstep_msgs.msg.Footstep.RARM: + offset = self.gg_param.i_param.leg_default_translate_pos.data[2*3+1] + elif leg == jsk_footstep_msgs.msg.Footstep.LARM: + offset = self.gg_param.i_param.leg_default_translate_pos.data[3*3+1] + mat = tf.transformations.quaternion_matrix([orientation.x, orientation.y, orientation.z, orientation.w]) + new_position = numpy.dot(mat, numpy.array([0, -1 * offset, 0, 0]))[:3] + numpy.array([position.x, position.y, position.z]) + ret.position = geometry_msgs.msg.Point(x=new_position[0], y=new_position[1], z=new_position[2]) + ret.orientation = orientation + return ret + + def _get_multi_gait_go_pos(self, start, goal): + mgg_msg = hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos() + diff_p = numpy.array([goal.position.x - start.position.x, + goal.position.y - start.position.y, + goal.position.z - start.position.z, + 0.0]) # dummy + start_mat = tf.transformations.quaternion_matrix([start.orientation.x, + start.orientation.y, + start.orientation.z, + start.orientation.w]) + goal_mat = tf.transformations.quaternion_matrix([goal.orientation.x, + goal.orientation.y, + goal.orientation.z, + goal.orientation.w]) + yaw = tf.transformations.euler_from_matrix(numpy.dot(start_mat.transpose(), goal_mat))[2] + mgg_msg.x = numpy.dot(start_mat.transpose(), diff_p)[0] + mgg_msg.y = numpy.dot(start_mat.transpose(), diff_p)[1] + mgg_msg.th = tf.transformations.math.degrees(yaw) + return mgg_msg + + + def _heat_color(self, v): + ratio = 2 * v + r = int(max(0.0, 255 * (ratio - 1.0))) + b = int(max(0, 255 * (1.0 - ratio))) + g = 255 - b - r + return [x / 255.0 for x in [r, g, b]] + + def main(self): + # wait for service + for i in ['/call_gait_state_event']: + rospy.wait_for_service(i) + # start + rospy.loginfo("[%s] start main loop", rospy.get_name()) + rospy.spin() + +if __name__ == "__main__": + ins = GaitPlannerClass() + ins.main() diff --git a/hrpsys_ros_bridge_jvrc/scripts/multi_gait_walk_node.py b/hrpsys_ros_bridge_jvrc/scripts/multi_gait_walk_node.py new file mode 100755 index 00000000..aa85535b --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/scripts/multi_gait_walk_node.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python + +import rospy, tf +import std_msgs.msg, jsk_footstep_msgs.msg, visualization_msgs.msg, geometry_msgs.msg, jsk_recognition_msgs.msg, hrpsys_ros_bridge_jvrc.msg +import hrpsys_ros_bridge.srv, drc_task_common.srv, hrpsys_ros_bridge_jvrc.srv + +class MultiGaitWalkingClass(object): + def __init__(self, node_name='multi_gait_walking_node'): + rospy.init_node(node_name) + # subscriber + rospy.Subscriber("/multi_go_pos_order", hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPosArray, self.multi_go_pos_order_callback) + rospy.Subscriber("/multi_gait_go_pos_command", std_msgs.msg.Empty, self.multi_gait_go_pos_callback) + # service proxy + self.go_pos = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/goPos', + hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_goPos) + self.wait_foot_steps = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/waitFootSteps', + hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_waitFootSteps) + self.move_quadruped = rospy.ServiceProxy('/move_quadruped', + hrpsys_ros_bridge_jvrc.srv.StringRequest) + self.move_biped = rospy.ServiceProxy('/move_biped', + hrpsys_ros_bridge_jvrc.srv.StringRequest) + self.get_abc_param = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/getAutoBalancerParam', + hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_getAutoBalancerParam) + self.fsm_call_srv = rospy.ServiceProxy('/call_gait_state_event', hrpsys_ros_bridge_jvrc.srv.StringRequest) + # keep order + self.multi_go_pos_order = None + + def multi_go_pos_order_callback(self, msg): + self.multi_go_pos_order = msg + rospy.logwarn("[%s] I've got order", rospy.get_name()) + + def multi_gait_go_pos_callback(self, msg): + if self.multi_go_pos_order == None: + rospy.logwarn("[%s] I do not have any order", rospy.get_name()) + return + for idx, order in enumerate(self.multi_go_pos_order.orders): + gait_type = self.get_abc_param().i_param.default_gait_type + first_event = 'biped walk' if gait_type == 0 else 'quadruped walk' + second_event = 'biped stop' if gait_type == 0 else 'quadruped stop' + first_res = self.fsm_call_srv(hrpsys_ros_bridge_jvrc.srv.StringRequestRequest(first_event)) + if first_res.result == False: + return + req = hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_goPosRequest(x=order.x, y=order.y, th=order.th) + self.go_pos(req) + rospy.loginfo("[%s] start go pos %s %s %s and waiting...", rospy.get_name(), order.x, order.y, order.th) + self.wait_foot_steps() + # check fsm + self.fsm_call_srv(hrpsys_ros_bridge_jvrc.srv.StringRequestRequest(second_event)) + rospy.loginfo("[%s] finish go pos %s %s %s", rospy.get_name(), order.x, order.y, order.th) + if idx != len(self.multi_go_pos_order.orders) - 1: + if gait_type == 0: # now biped + trans_res = self.move_quadruped(hrpsys_ros_bridge_jvrc.srv.StringRequestRequest("")) + if trans_res == False: + return + elif gait_type == 1: # now quadruped + trans_res = self.move_biped(hrpsys_ros_bridge_jvrc.srv.StringRequestRequest("")) + if trans_res == False: + return + else: + self.multi_go_pos_order = None + + def main(self): + # wait for service + for i in ['/AutoBalancerServiceROSBridge/goPos', + '/AutoBalancerServiceROSBridge/waitFootSteps', + '/move_quadruped', + '/move_biped']: + rospy.wait_for_service(i) + # start + rospy.loginfo("[%s] start main loop", rospy.get_name()) + rospy.spin() + +if __name__ == "__main__": + ins = MultiGaitWalkingClass() + ins.main() diff --git a/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py index 9a868b76..76cb0835 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import rospy, tf -import std_msgs.msg +import std_msgs.msg, jsk_footstep_msgs.msg, visualization_msgs.msg, geometry_msgs.msg, jsk_recognition_msgs.msg import hrpsys_ros_bridge.srv, drc_task_common.srv, hrpsys_ros_bridge_jvrc.srv class MultiWalkingClass(object): @@ -10,8 +10,11 @@ def __init__(self, node_name='multi_walking_node'): # param self.reference_frame_id = rospy.get_param("~reference_frame_id", "ground") self.target_frame_id = rospy.get_param("~target_frame_id", "robot_marker_root") + # Publisher + self.go_pos_footsteps_pub = rospy.Publisher('/go_pos_footsteps', jsk_footstep_msgs.msg.FootstepArray, queue_size=1) # subscriber rospy.Subscriber("/go_pos_command", std_msgs.msg.Empty, self.go_pos_callback) + rospy.Subscriber("/urdf_control_marker/feedback", visualization_msgs.msg.InteractiveMarkerFeedback, self.marker_callback) # service proxy self.go_pos = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/goPos', hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_goPos) @@ -19,10 +22,18 @@ def __init__(self, node_name='multi_walking_node'): hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_waitFootSteps) self.get_abc_param = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/getAutoBalancerParam', hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_getAutoBalancerParam) + self.get_go_pos_footsteps = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/getGoPosFootstepsSequence', + hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_getGoPosFootstepsSequence) self.menu_call_srv = rospy.ServiceProxy('/rviz_menu_call', drc_task_common.srv.RvizMenuCall) self.fsm_call_srv = rospy.ServiceProxy('/call_gait_state_event', hrpsys_ros_bridge_jvrc.srv.StringRequest) # tf self.tfl = tf.TransformListener() + # for marker_callback + self.prev_event_type = 0 + # get vertices + rospy.wait_for_service("/StabilizerServiceROSBridge/getParameter") + self.st_param = rospy.ServiceProxy("/StabilizerServiceROSBridge/getParameter", hrpsys_ros_bridge.srv.OpenHRP_StabilizerService_getParameter)() + # st_param.i_param.eefm_support_polygon_vertices_sequence[0].vertices[0].pos -> (0.182, 0.07112) def go_pos_callback(self, msg): ts = rospy.Time.now() @@ -57,10 +68,57 @@ def go_pos_callback(self, msg): self.fsm_call_srv(hrpsys_ros_bridge_jvrc.srv.StringRequestRequest(second_event)) rospy.loginfo("[%s] finish go pos %s %s %s", rospy.get_name(), x, y, yaw) + def marker_callback(self, msg): + cur_event_type = msg.event_type + if self.prev_event_type == 1 and cur_event_type == 5: + ts = rospy.Time.now() + try: + self.tfl.waitForTransform(self.reference_frame_id, self.target_frame_id, ts, rospy.Duration(1)) + (trans, rot) = self.tfl.lookupTransform(self.reference_frame_id, self.target_frame_id, ts) + except: + rospy.logwarn("[%s] failed to solve tf from %s to %s", rospy.get_name(), self.reference_frame_id, self.target_frame_id) + return + x = trans[0] + y = trans[1] + yaw = tf.transformations.math.degrees(tf.transformations.euler_from_quaternion(rot)[2]) + req = hrpsys_ros_bridge.srv.OpenHRP_AutoBalancerService_getGoPosFootstepsSequenceRequest(x=x, y=y, th=yaw) + res = self.get_go_pos_footsteps(req) + if res.operation_return == True: + fsll_msg = self._hrpsys_footsteps_to_jsk_footstep_msgs(res.o_footstep, msg.header.stamp) + self.go_pos_footsteps_pub.publish(fsll_msg) + self.prev_event_type = cur_event_type + + def _hrpsys_footsteps_to_jsk_footstep_msgs(self, hrpsys_footsteps, ts): + fsll_msg = jsk_footstep_msgs.msg.FootstepArray() + fsll_msg.header.stamp = ts + fsll_msg.header.frame_id = "odom" + for idx, fsl in enumerate(hrpsys_footsteps): + for fs in fsl.fs: + fsl_msg = jsk_footstep_msgs.msg.Footstep() + if fs.leg == "rleg": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.RLEG + elif fs.leg == "lleg": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.LLEG + elif fs.leg == "rarm": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.RARM + elif fs.leg == "larm": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.LARM + fsl_msg.pose = geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=fs.pos[0], y=fs.pos[1], z=fs.pos[2]), + orientation=geometry_msgs.msg.Quaternion(w=fs.rot[0], x=fs.rot[1], y=fs.rot[2], z=fs.rot[3])) + i_param = self.st_param.i_param + fsl_msg.dimensions.x = i_param.eefm_leg_front_margin + i_param.eefm_leg_rear_margin + fsl_msg.dimensions.y = i_param.eefm_leg_inside_margin + i_param.eefm_leg_outside_margin + fsl_msg.dimensions.z = 0.05 + fsl_msg.cost = float(idx) # order for debug + fsll_msg.footsteps.append(fsl_msg) + return fsll_msg + def main(self): # wait for service for i in ['/AutoBalancerServiceROSBridge/goPos', '/AutoBalancerServiceROSBridge/waitFootSteps', + '/AutoBalancerServiceROSBridge/getAutoBalancerParam', + '/AutoBalancerServiceROSBridge/getGoPosFootstepsSequence', '/rviz_menu_call', '/call_gait_state_event']: rospy.wait_for_service(i)