From d0c67f028d9cce782572ce3ce416741b1bbe8931 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Mon, 4 Jan 2016 13:16:07 +0900 Subject: [PATCH 01/22] [hrpsys_ros_bridge_jvrc] add jaxon_jvrc_pointcloud_minimal.launch --- .../config/jaxon_jvrc_self_filter.yaml | 80 +++++++++++++++++++ .../jaxon_jvrc_pointcloud_minimal.launch | 66 +++++++++++++++ 2 files changed, 146 insertions(+) create mode 100644 hrpsys_ros_bridge_jvrc/config/jaxon_jvrc_self_filter.yaml create mode 100644 hrpsys_ros_bridge_jvrc/launch/jaxon_jvrc_pointcloud_minimal.launch 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/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 + + + From 0c8e0faa9f2c2be01e4cc5b9f2608d39c677ed96 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Thu, 7 Jan 2016 19:27:21 +0900 Subject: [PATCH 02/22] [multi_gait.launch] odom_on_ground is deprecated in real robot --- hrpsys_ros_bridge_jvrc/config/multi_gait.rviz | 373 +++++++++++------- .../launch/multi_gait.launch | 2 +- 2 files changed, 236 insertions(+), 139 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index 3dfdfd6d..625369f7 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -3,9 +3,11 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.570997 - Tree Height: 463 + Expanded: + - /Global Options1 + - /LaserScan1/Status1 + Splitter Ratio: 0.413567 + Tree Height: 1135 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -24,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: LaserScan Visualization Manager: Class: "" Displays: @@ -46,6 +48,113 @@ Visualization Manager: Plane Cell Count: 10 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/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + 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 + 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: 4103 + Min Color: 0; 0; 0 + Min Intensity: 47 + 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 +194,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 +264,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 +350,141 @@ 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 - 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 - 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: 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 - 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: true - Show Visual Aids: false - 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 Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: odom_on_ground + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -415,23 +512,23 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 0.191248 - Y: -0.811557 - Z: 0.790978 + X: 0.378797 + Y: -0.67346 + Z: 0.449261 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.645398 + Pitch: 0.610398 Target Frame: Value: Orbit (rviz) - Yaw: 2.6754 + Yaw: 3.08358 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 744 - Hide Left Dock: true + collapsed: false + Height: 1416 + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002a80000025efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000025e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005560000003efc0100000002fb0000000800540069006d0065010000000000000556000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005560000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a4000004fefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004fe000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000656000004fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -440,6 +537,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1366 - X: 0 - Y: 24 + Width: 2560 + X: -8 + Y: -6 diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index ae7094f2..28c46954 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -3,7 +3,7 @@ - + From ebc297bee62d3760ea71e990498db3201abff082 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 9 Jan 2016 06:48:17 +0900 Subject: [PATCH 03/22] [hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py] visualize go pos foot steps --- .../scripts/simple_walk_node.py | 57 ++++++++++++++++++- 1 file changed, 56 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py index 9a868b76..6396a23a 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 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,14 @@ 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 def go_pos_callback(self, msg): ts = rospy.Time.now() @@ -57,10 +64,58 @@ 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 = jsk_footstep_msgs.msg.FootstepArray() + fsll_msg.header.stamp = msg.header.stamp + fsll_msg.header.frame_id = "odom" + fsll = res.o_footstep + for i in range(len(fsll)): + fsl = fsll[i] + for j in range(len(fsl.fs)): + fsl_msg = jsk_footstep_msgs.msg.Footstep() + if fsl.fs[j].leg == "rleg": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.RLEG + elif fsl.fs[j].leg == "lleg": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.LLEG + elif fsl.fs[j].leg == "rarm": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.RARM + elif fsl.fs[j].leg == "larm": + fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.LARM + pose_msg = geometry_msgs.msg.Pose() + pose_msg.position.x = fsl.fs[j].pos[0] + pose_msg.position.y = fsl.fs[j].pos[1] + pose_msg.position.z = fsl.fs[j].pos[2] + pose_msg.orientation.w = fsl.fs[j].rot[0] + pose_msg.orientation.x = fsl.fs[j].rot[1] + pose_msg.orientation.y = fsl.fs[j].rot[2] + pose_msg.orientation.z = fsl.fs[j].rot[3] + fsl_msg.pose = pose_msg + fsll_msg.footsteps.append(fsl_msg) + self.go_pos_footsteps_pub.publish(fsll_msg) + self.prev_event_type = cur_event_type + + 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) From 7e21065adc416e2fe224a26dee0bcdabbb284f82 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Sat, 9 Jan 2016 06:48:43 +0900 Subject: [PATCH 04/22] [multi_gait.rviz] display footsteps --- hrpsys_ros_bridge_jvrc/config/multi_gait.rviz | 168 +++++------------- 1 file changed, 42 insertions(+), 126 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index 625369f7..ef544a6e 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -5,9 +5,9 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /LaserScan1/Status1 + - /Footstep1 Splitter Ratio: 0.413567 - Tree Height: 1135 + Tree Height: 463 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -92,9 +92,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4103 + Max Intensity: 1000 Min Color: 0; 0; 0 - Min Intensity: 47 + Min Intensity: 1000 Name: LaserScan Position Transformer: XYZ Queue Size: 10 @@ -194,6 +194,16 @@ 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 @@ -264,22 +274,22 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - L_thk_finger1: + Link Tree Style: Links in Alphabetic Order + RANGE_LINK: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_thk_finger2: + RARM_FINGER0: Alpha: 1 Show Axes: false Show Trail: false Value: true - L_thk_palm: + RARM_FINGER1: Alpha: 1 Show Axes: false Show Trail: false Value: true - Link Tree Style: Links in Alphabetic Order RARM_LINK0: Alpha: 1 Show Axes: false @@ -350,137 +360,43 @@ 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: 0.5 + Class: jsk_rviz_plugin/Footstep + Enabled: true + Name: Footstep + Show Name: true + Topic: /go_pos_footsteps + Use Group Coloring: false + Value: true + Width: 0.15 + depth: 0.3 + height: 0.01 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -505,30 +421,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 5.89461 + Distance: 10.2691 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.378797 - Y: -0.67346 - Z: 0.449261 + X: -0.39312 + Y: -1.76254 + Z: 0.261527 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.610398 + Pitch: 0.375398 Target Frame: Value: Orbit (rviz) - Yaw: 3.08358 + Yaw: 2.40858 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 1416 - Hide Left Dock: false + collapsed: true + Height: 744 + Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000003a4000004fefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004fe000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000656000004fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a40000025efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000025e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005560000003efc0100000002fb0000000800540069006d0065010000000000000556000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005560000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -537,6 +453,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2560 - X: -8 - Y: -6 + Width: 1366 + X: 0 + Y: 24 From 112ef3fba6b4355a2ec1a6e33007171ae53de815 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Mon, 11 Jan 2016 18:41:41 +0900 Subject: [PATCH 05/22] [simple_walk_node.py] publish bounding box around footstep --- .../scripts/simple_walk_node.py | 78 ++++++++++++------- 1 file changed, 51 insertions(+), 27 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py index 6396a23a..e2fdab39 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, jsk_footstep_msgs.msg, visualization_msgs.msg, geometry_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): @@ -12,6 +12,7 @@ def __init__(self, node_name='multi_walking_node'): 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) + self.go_pos_footsteps_bb_pub = rospy.Publisher('/go_pos_footsteps_boundingbox', jsk_recognition_msgs.msg.BoundingBoxArray, 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) @@ -30,6 +31,10 @@ def __init__(self, node_name='multi_walking_node'): 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() @@ -80,35 +85,54 @@ def marker_callback(self, msg): 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 = jsk_footstep_msgs.msg.FootstepArray() - fsll_msg.header.stamp = msg.header.stamp - fsll_msg.header.frame_id = "odom" - fsll = res.o_footstep - for i in range(len(fsll)): - fsl = fsll[i] - for j in range(len(fsl.fs)): - fsl_msg = jsk_footstep_msgs.msg.Footstep() - if fsl.fs[j].leg == "rleg": - fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.RLEG - elif fsl.fs[j].leg == "lleg": - fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.LLEG - elif fsl.fs[j].leg == "rarm": - fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.RARM - elif fsl.fs[j].leg == "larm": - fsl_msg.leg = jsk_footstep_msgs.msg.Footstep.LARM - pose_msg = geometry_msgs.msg.Pose() - pose_msg.position.x = fsl.fs[j].pos[0] - pose_msg.position.y = fsl.fs[j].pos[1] - pose_msg.position.z = fsl.fs[j].pos[2] - pose_msg.orientation.w = fsl.fs[j].rot[0] - pose_msg.orientation.x = fsl.fs[j].rot[1] - pose_msg.orientation.y = fsl.fs[j].rot[2] - pose_msg.orientation.z = fsl.fs[j].rot[3] - fsl_msg.pose = pose_msg - fsll_msg.footsteps.append(fsl_msg) + fsll_msg = self._hrpsys_footsteps_to_jsk_footstep_msgs(res.o_footstep, msg.header.stamp) self.go_pos_footsteps_pub.publish(fsll_msg) + bb_msg = self._jsk_footstep_msgs_to_bounding_box_array(fsll_msg) + self.go_pos_footsteps_bb_pub.publish(bb_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 fsl in 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 + fsll_msg.footsteps.append(fsl_msg) + return fsll_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.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 main(self): # wait for service From 9a6bac0fad19826e5d43f2d5fb2a9a19d35cc4ff Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Mon, 11 Jan 2016 18:42:09 +0900 Subject: [PATCH 06/22] [multi_gait.rviz] update rviz config to visualize bounding box --- hrpsys_ros_bridge_jvrc/config/multi_gait.rviz | 254 ++++++++++++++---- 1 file changed, 196 insertions(+), 58 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index ef544a6e..a1bbc586 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -3,11 +3,9 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Footstep1 + Expanded: ~ Splitter Ratio: 0.413567 - Tree Height: 463 + Tree Height: 1135 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -48,35 +46,6 @@ Visualization Manager: Plane Cell Count: 10 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/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - 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 - Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -92,9 +61,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 1000 + Max Intensity: 3956 Min Color: 0; 0; 0 - Min Intensity: 1000 + Min Intensity: 44 Name: LaserScan Position Transformer: XYZ Queue Size: 10 @@ -194,16 +163,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 @@ -274,22 +233,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 @@ -360,26 +319,131 @@ 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: "" @@ -397,6 +461,79 @@ Visualization Manager: Width: 0.15 depth: 0.3 height: 0.01 + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: 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: + - 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 + 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.01 + 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: 8.02193 + Min Value: -2.5261 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + 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.01 + Style: Flat Squares + Topic: /full_cloud2 + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: PointCloudGroup Enabled: true Global Options: Background Color: 48; 48; 48 @@ -417,34 +554,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: 10.2691 + Distance: 10.6284 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.39312 - Y: -1.76254 - Z: 0.261527 + X: 0.0333784 + Y: -1.06746 + Z: 0.495033 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.375398 + Pitch: 0.504797 Target Frame: Value: Orbit (rviz) - Yaw: 2.40858 + Yaw: 2.06855 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 744 + Height: 1416 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000003a40000025efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000025e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005560000003efc0100000002fb0000000800540069006d0065010000000000000556000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005560000025e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a5000004fefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000004fe000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000a00000004fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -453,6 +591,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1366 + Width: 2560 X: 0 Y: 24 From e15b5591b62a644e5523778e9954d7959a50322d Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Mon, 11 Jan 2016 21:35:15 +0900 Subject: [PATCH 07/22] [simple_walk_node.py, gait_planner_node.py] add gait_planner_node to decide which gait type --- .../scripts/gait_planner_node.py | 52 +++++++++++++++++++ .../scripts/simple_walk_node.py | 22 -------- 2 files changed, 52 insertions(+), 22 deletions(-) create mode 100755 hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py 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..20b61076 --- /dev/null +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -0,0 +1,52 @@ +#!/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, sensor_msgs.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) + # Publisher + self.go_pos_footsteps_bb_pub = rospy.Publisher('/go_pos_footsteps_boundingbox', jsk_recognition_msgs.msg.BoundingBoxArray, queue_size=1) + # subscriber + rospy.Subscriber('/go_pos_footsteps', jsk_footstep_msgs.msg.FootstepArray, self.footstep_callback) + rospy.Subscriber('/latest_complete_heightmap/output', sensor_msgs.msg.Image, self.footstep_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)() + + 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) + + 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.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 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/simple_walk_node.py b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py index e2fdab39..4821a3d8 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py @@ -12,7 +12,6 @@ def __init__(self, node_name='multi_walking_node'): 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) - self.go_pos_footsteps_bb_pub = rospy.Publisher('/go_pos_footsteps_boundingbox', jsk_recognition_msgs.msg.BoundingBoxArray, 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) @@ -87,8 +86,6 @@ def marker_callback(self, msg): 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) - bb_msg = self._jsk_footstep_msgs_to_bounding_box_array(fsll_msg) - self.go_pos_footsteps_bb_pub.publish(bb_msg) self.prev_event_type = cur_event_type def _hrpsys_footsteps_to_jsk_footstep_msgs(self, hrpsys_footsteps, ts): @@ -115,25 +112,6 @@ def _hrpsys_footsteps_to_jsk_footstep_msgs(self, hrpsys_footsteps, ts): fsll_msg.footsteps.append(fsl_msg) return fsll_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.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 main(self): # wait for service for i in ['/AutoBalancerServiceROSBridge/goPos', From 9dbbd94c50322034fc2d6ef4ee2bec7a00f304d1 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Mon, 11 Jan 2016 21:39:26 +0900 Subject: [PATCH 08/22] [multi_gait.launch] use gait_planner_node --- hrpsys_ros_bridge_jvrc/launch/multi_gait.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index 28c46954..5ba00dee 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -41,6 +41,7 @@ + From 471e649ee0aa00bf2a776253dc032507e11aeb34 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Tue, 12 Jan 2016 22:14:27 +0900 Subject: [PATCH 09/22] [gait_planner_node.py] calculate standard deviation inside foot step --- .../launch/multi_gait.launch | 4 +- .../scripts/gait_planner_node.py | 86 ++++++++++++++++++- 2 files changed, 87 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index 5ba00dee..fd548f5f 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -41,7 +41,9 @@ - + + + diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py index 20b61076..5fb9131b 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -1,6 +1,7 @@ #!/usr/bin/env python -import rospy, tf +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 import hrpsys_ros_bridge.srv, drc_task_common.srv, hrpsys_ros_bridge_jvrc.srv @@ -9,16 +10,51 @@ def __init__(self, node_name='gait_planner_node'): rospy.init_node(node_name) # Publisher self.go_pos_footsteps_bb_pub = rospy.Publisher('/go_pos_footsteps_boundingbox', jsk_recognition_msgs.msg.BoundingBoxArray, queue_size=1) + self.go_pos_footsteps_bb_pub2 = rospy.Publisher('/go_pos_footsteps_boundingbox2', jsk_recognition_msgs.msg.BoundingBoxArray, queue_size=1) + self.masked_heightmap_pub = rospy.Publisher('/masked_heightmap', sensor_msgs.msg.Image, queue_size=1) # subscriber rospy.Subscriber('/go_pos_footsteps', jsk_footstep_msgs.msg.FootstepArray, self.footstep_callback) - rospy.Subscriber('/latest_complete_heightmap/output', sensor_msgs.msg.Image, self.footstep_callback) + rospy.Subscriber('~input/heightmap', sensor_msgs.msg.Image, self.heightmap_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)() + # tf + self.tfl = tf.TransformListener() + # store heightmap + self.bridge = cv_bridge.CvBridge() + self.my_heightmap_header = None + self.my_heightmap = None 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) + self.go_pos_footsteps_bb_pub2.publish(transformed_bb_msg) + vertices_list = self._vertices_of_bounding_box_array(transformed_bb_msg) + x_min = rospy.get_param("/latest_heightmap/min_x") + x_max = rospy.get_param("/latest_heightmap/max_x") + y_min = rospy.get_param("/latest_heightmap/min_y") + y_max = rospy.get_param("/latest_heightmap/max_y") + x_pix = rospy.get_param("/latest_heightmap/resolution_x") + y_pix = rospy.get_param("/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) + for vs in vertices_list_in_pixel: + 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("minMaxLoc : %s", cv2.minMaxLoc(self.my_heightmap, mask=mask_image)) + rospy.logwarn("meanStdDev : %s", cv2.meanStdDev(self.my_heightmap, mask=mask_image)) + + 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 _jsk_footstep_msgs_to_bounding_box_array(self, fsl): box_array = jsk_recognition_msgs.msg.BoundingBoxArray() @@ -39,6 +75,52 @@ def _jsk_footstep_msgs_to_bounding_box_array(self, fsl): 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 _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']: From c276c86029057a256ec36c8d4ed215ca68d774bd Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Tue, 12 Jan 2016 23:49:49 +0900 Subject: [PATCH 10/22] [gait_planner_node.py] subscribe contact place canidate for 3-leg walk --- hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py index 5fb9131b..0029816f 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -10,11 +10,12 @@ def __init__(self, node_name='gait_planner_node'): rospy.init_node(node_name) # Publisher self.go_pos_footsteps_bb_pub = rospy.Publisher('/go_pos_footsteps_boundingbox', jsk_recognition_msgs.msg.BoundingBoxArray, queue_size=1) - self.go_pos_footsteps_bb_pub2 = rospy.Publisher('/go_pos_footsteps_boundingbox2', jsk_recognition_msgs.msg.BoundingBoxArray, queue_size=1) self.masked_heightmap_pub = rospy.Publisher('/masked_heightmap', sensor_msgs.msg.Image, 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)() @@ -24,6 +25,8 @@ def __init__(self, node_name='gait_planner_node'): self.bridge = cv_bridge.CvBridge() self.my_heightmap_header = None self.my_heightmap = None + # store contact plane candidate + self.contact_plane_candidate = None def footstep_callback(self, msg): bb_msg = self._jsk_footstep_msgs_to_bounding_box_array(msg) @@ -33,7 +36,6 @@ def footstep_callback(self, msg): return else: transformed_bb_msg = self._tf_transform_bounding_box_array(bb_msg) - self.go_pos_footsteps_bb_pub2.publish(transformed_bb_msg) vertices_list = self._vertices_of_bounding_box_array(transformed_bb_msg) x_min = rospy.get_param("/latest_heightmap/min_x") x_max = rospy.get_param("/latest_heightmap/max_x") @@ -56,6 +58,9 @@ 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 From a8389ea561423a331e863d51a2ec76a8d48fc1cf Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Wed, 13 Jan 2016 00:30:22 +0900 Subject: [PATCH 11/22] [msg/MultiGaitGoPos.msg] add MultiGaitGoPos.msg --- hrpsys_ros_bridge_jvrc/CMakeLists.txt | 5 +++++ hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPos.msg | 11 +++++++++++ 2 files changed, 16 insertions(+) create mode 100644 hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPos.msg diff --git a/hrpsys_ros_bridge_jvrc/CMakeLists.txt b/hrpsys_ros_bridge_jvrc/CMakeLists.txt index 43e5889d..fd6af3d6 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 + ) + add_service_files( DIRECTORY srv FILES StringRequest.srv 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 From 1bad42e8b6eb7b6b4aa939ea89166bf5ae51b8eb Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Wed, 13 Jan 2016 00:37:28 +0900 Subject: [PATCH 12/22] [msg/MultiGaitGoPosArray.msg] add MultiGaitGoPosArray.msg --- hrpsys_ros_bridge_jvrc/CMakeLists.txt | 2 +- hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPosArray.msg | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 hrpsys_ros_bridge_jvrc/msg/MultiGaitGoPosArray.msg diff --git a/hrpsys_ros_bridge_jvrc/CMakeLists.txt b/hrpsys_ros_bridge_jvrc/CMakeLists.txt index fd6af3d6..2ee26475 100644 --- a/hrpsys_ros_bridge_jvrc/CMakeLists.txt +++ b/hrpsys_ros_bridge_jvrc/CMakeLists.txt @@ -13,7 +13,7 @@ pkg_check_modules(hrpsys hrpsys-base REQUIRED) add_message_files( DIRECTORY msg - FILES MultiGaitGoPos.msg + FILES MultiGaitGoPos.msg MultiGaitGoPosArray.msg ) add_service_files( 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 From ec8baca75d194e8447c7dd44d936355ebe44c76f Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Wed, 13 Jan 2016 02:13:23 +0900 Subject: [PATCH 13/22] [gait_planner_node.py] publish MultiGaitGoPosArray according to foot print standard deviation --- .../scripts/gait_planner_node.py | 75 ++++++++++++++++++- 1 file changed, 73 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py index 0029816f..4e24ae5a 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -2,7 +2,7 @@ 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 +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): @@ -11,6 +11,7 @@ def __init__(self, node_name='gait_planner_node'): # 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) @@ -19,6 +20,9 @@ def __init__(self, node_name='gait_planner_node'): 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 # tf self.tfl = tf.TransformListener() # store heightmap @@ -47,12 +51,39 @@ def footstep_callback(self, msg): # 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) - for vs in vertices_list_in_pixel: + # 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) + 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("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.3 and 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 + elif stddev[0,0] < 0.3 and gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.TROT: + 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 + 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 @@ -119,6 +150,46 @@ def _meter_to_pixel(self, vertices_list, x_min, x_max, y_min, y_max, x_pix, y_pi 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, diff_p)[0] + mgg_msg.y = numpy.dot(start_mat, 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))) From a83d278d8f888c8a6b69463ec971edf84304f338 Mon Sep 17 00:00:00 2001 From: eisoku9618 Date: Wed, 13 Jan 2016 03:41:06 +0900 Subject: [PATCH 14/22] [multi_gait_walk_node.py] add multi_gait_walk_node.py to realize multi gait walk --- .../euslisp/robot_controller_node.l | 29 +++++++ .../launch/multi_gait.launch | 1 + .../scripts/gait_planner_node.py | 9 ++- .../scripts/multi_gait_walk_node.py | 75 +++++++++++++++++++ .../scripts/simple_walk_node.py | 3 +- 5 files changed, 112 insertions(+), 5 deletions(-) create mode 100755 hrpsys_ros_bridge_jvrc/scripts/multi_gait_walk_node.py diff --git a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l index fdec6ebc..0fdc291a 100755 --- a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l +++ b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l @@ -41,6 +41,35 @@ (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) diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index fd548f5f..08fc2ea3 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -44,6 +44,7 @@ + diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py index 4e24ae5a..048c673a 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -61,8 +61,8 @@ def footstep_callback(self, msg): 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("minMaxLoc : %s", cv2.minMaxLoc(self.my_heightmap, mask=mask_image)) - rospy.logwarn("meanStdDev : %s", cv2.meanStdDev(self.my_heightmap, mask=mask_image)) + # 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.3 and gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.BIPED: mgg_msg = self._get_multi_gait_go_pos(origin, prev_origin) @@ -98,6 +98,7 @@ def _jsk_footstep_msgs_to_bounding_box_array(self, fsl): 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 @@ -184,8 +185,8 @@ def _get_multi_gait_go_pos(self, start, goal): 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, diff_p)[0] - mgg_msg.y = numpy.dot(start_mat, diff_p)[1] + 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 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 4821a3d8..76cb0835 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/simple_walk_node.py @@ -92,7 +92,7 @@ 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 fsl in hrpsys_footsteps: + for idx, fsl in enumerate(hrpsys_footsteps): for fs in fsl.fs: fsl_msg = jsk_footstep_msgs.msg.Footstep() if fs.leg == "rleg": @@ -109,6 +109,7 @@ def _hrpsys_footsteps_to_jsk_footstep_msgs(self, hrpsys_footsteps, ts): 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 From 41a5749388cd34a0bc8afe1f017e3cf9741dd1b8 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Thu, 21 Jan 2016 07:31:41 +0900 Subject: [PATCH 15/22] [hrpsys_ros_bridge_jvrc] add polygon_select.launch to get polygon(plane) by clicking in rviz --- .../launch/polygon_select.launch | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 hrpsys_ros_bridge_jvrc/launch/polygon_select.launch 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..6f322c2a --- /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: -1.0 + filter_limit_max: 2.0 + + + + + + + + k_search: 50 + + + + + + + + max_area: 1.0 + min_area: 0.1 + + + + + + + + + + From c48fba4b1584c375d9a8d194842bb9ca0d5935a6 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Thu, 21 Jan 2016 07:54:37 +0900 Subject: [PATCH 16/22] [robot_controller_node.l, multi_gait.launch] store selected polygon in RViz --- hrpsys_ros_bridge_jvrc/config/multi_gait.rviz | 41 +++++++++++++++---- .../euslisp/robot_controller_node.l | 22 +++++++++- .../launch/multi_gait.launch | 1 + 3 files changed, 55 insertions(+), 9 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index a1bbc586..846786f1 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -3,7 +3,10 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /select-contact-plane1 + - /select-contact-plane1/PolygonArray1 + - /select-contact-plane1/QuietInteractiveMarker1 Splitter Ratio: 0.413567 Tree Height: 1135 - Class: rviz/Selection @@ -61,9 +64,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 3956 + Max Intensity: 3557 Min Color: 0; 0; 0 - Min Intensity: 44 + Min Intensity: 62 Name: LaserScan Position Transformer: XYZ Queue Size: 10 @@ -506,8 +509,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 8.02193 - Min Value: -2.5261 + Max Value: 6.79328 + Min Value: -0.940877 Value: true Axis: Z Channel Name: intensity @@ -534,6 +537,30 @@ Visualization Manager: Value: true Enabled: true Name: PointCloudGroup + - Class: rviz/Group + Displays: + - Alpha: 1 + Class: jsk_rviz_plugin/PolygonArray + Color: 25; 255; 0 + Enabled: true + 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 + Name: QuietInteractiveMarker + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /polygon_select_group/polygon_interactive_marker/update + Value: true + Enabled: true + Name: select-contact-plane Enabled: true Global Options: Background Color: 48; 48; 48 @@ -574,7 +601,7 @@ Visualization Manager: Pitch: 0.504797 Target Frame: Value: Orbit (rviz) - Yaw: 2.06855 + Yaw: 2.07855 Saved: ~ Window Geometry: Displays: @@ -592,5 +619,5 @@ Window Geometry: Views: collapsed: true Width: 2560 - X: 0 + X: -7 Y: 24 diff --git a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l index 0fdc291a..d8bc670b 100755 --- a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l +++ b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l @@ -12,7 +12,8 @@ :slots (robot-name tfl reference-frame-id use-robot-interface-flag potentio-vector joint-name-list - mg) + mg + selected-polygon-relative-to-body) ) (defmethod robot-menu @@ -22,7 +23,8 @@ (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)) @@ -37,6 +39,7 @@ (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) @@ -205,6 +208,21 @@ (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))) + )) ) ;; main diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index 08fc2ea3..1c586dcb 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -25,6 +25,7 @@ + From 4c7600a831a91f68a46d6de3ed1f721fd634932d Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Thu, 21 Jan 2016 11:56:43 +0900 Subject: [PATCH 17/22] [robot_controller_node.l, polygon_select.launch] solve ik to selected polygon --- hrpsys_ros_bridge_jvrc/config/multi_gait.rviz | 34 ++++---- .../euslisp/robot_controller_node.l | 86 ++++++++++++++++++- .../launch/polygon_select.launch | 4 +- 3 files changed, 103 insertions(+), 21 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index 846786f1..e3fbcddd 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -4,11 +4,13 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /PointCloudGroup1 + - /PointCloudGroup1/laser1 - /select-contact-plane1 - /select-contact-plane1/PolygonArray1 - /select-contact-plane1/QuietInteractiveMarker1 Splitter Ratio: 0.413567 - Tree Height: 1135 + Tree Height: 1107 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -64,9 +66,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 3557 + Max Intensity: 4203 Min Color: 0; 0; 0 - Min Intensity: 62 + Min Intensity: 72 Name: LaserScan Position Transformer: XYZ Queue Size: 10 @@ -509,8 +511,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 6.79328 - Min Value: -0.940877 + Max Value: 6.57932 + Min Value: -0.050695 Value: true Axis: Z Channel Name: intensity @@ -529,7 +531,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.01 + Size (m): 0.005 Style: Flat Squares Topic: /full_cloud2 Use Fixed Frame: true @@ -586,30 +588,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10.6284 + Distance: 2.40202 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0333784 - Y: -1.06746 - Z: 0.495033 + X: 1.42976 + Y: 0.598739 + Z: 1.27833 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.504797 + Pitch: 0.0447975 Target Frame: Value: Orbit (rviz) - Yaw: 2.07855 + Yaw: 0.488554 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1416 + Height: 1388 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000003a5000004fefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000004fe000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000a00000004fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a5000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000004e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d90000003efc0100000002fb0000000800540069006d00650100000000000005d9000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005d9000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -618,6 +620,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2560 - X: -7 + Width: 1497 + X: -6 Y: 24 diff --git a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l index d8bc670b..a06cde50 100755 --- a/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l +++ b/hrpsys_ros_bridge_jvrc/euslisp/robot_controller_node.l @@ -13,7 +13,8 @@ use-robot-interface-flag potentio-vector joint-name-list mg - selected-polygon-relative-to-body) + selected-polygon-relative-to-body + tripedal-av) ) (defmethod robot-menu @@ -30,8 +31,10 @@ (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) @@ -138,6 +141,7 @@ ":reset-pose" ":reset-manip-pose" ":init-pose" + ":tripedal-pose" ":quadruped-pose")) (req (instance drc_task_common::RvizMenuCallRequest :init @@ -152,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))) ) @@ -222,6 +230,77 @@ 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)))) + ) + ) )) ) @@ -236,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/polygon_select.launch b/hrpsys_ros_bridge_jvrc/launch/polygon_select.launch index 6f322c2a..3724c31d 100644 --- a/hrpsys_ros_bridge_jvrc/launch/polygon_select.launch +++ b/hrpsys_ros_bridge_jvrc/launch/polygon_select.launch @@ -43,7 +43,7 @@ filter_field_name: z - filter_limit_min: -1.0 + filter_limit_min: 0.8 filter_limit_max: 2.0 @@ -61,7 +61,7 @@ max_area: 1.0 - min_area: 0.1 + min_area: 0.01 From 0187f318e79d2bd79f325c5d210c10eb3e7cf355 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Fri, 22 Jan 2016 00:28:25 +0900 Subject: [PATCH 18/22] [multi_gait.launch, gait_planner_node.py] fix bug : parameter + order of initialize --- .../launch/multi_gait.launch | 3 +- .../scripts/gait_planner_node.py | 36 +++++++++---------- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch index 1c586dcb..ee6a65e2 100644 --- a/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch +++ b/hrpsys_ros_bridge_jvrc/launch/multi_gait.launch @@ -26,6 +26,7 @@ + @@ -43,7 +44,7 @@ - + diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py index 048c673a..8f192d06 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -8,6 +8,14 @@ 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) @@ -23,14 +31,6 @@ def __init__(self, node_name='gait_planner_node'): 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 - # 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 def footstep_callback(self, msg): bb_msg = self._jsk_footstep_msgs_to_bounding_box_array(msg) @@ -41,12 +41,12 @@ def footstep_callback(self, msg): 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("/latest_heightmap/min_x") - x_max = rospy.get_param("/latest_heightmap/max_x") - y_min = rospy.get_param("/latest_heightmap/min_y") - y_max = rospy.get_param("/latest_heightmap/max_y") - x_pix = rospy.get_param("/latest_heightmap/resolution_x") - y_pix = rospy.get_param("/latest_heightmap/resolution_y") + 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) @@ -61,16 +61,16 @@ def footstep_callback(self, msg): 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("minMaxLoc : %s", cv2.minMaxLoc(self.my_heightmap, mask=mask_image)) - # rospy.logwarn("meanStdDev : %s", cv2.meanStdDev(self.my_heightmap, mask=mask_image)) + 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.3 and gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.BIPED: + if stddev[0,0] > 0.01 and 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 - elif stddev[0,0] < 0.3 and gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.TROT: + elif stddev[0,0] < 0.01 and gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.TROT: new_origin = self._calc_reference_pose(fs) mgg_msg = self._get_multi_gait_go_pos(origin, new_origin) mgg_msg.gait_type = gait_type From 433687b9c7a314eafaaec41ba4cdd0ce5750fa37 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Fri, 22 Jan 2016 01:31:02 +0900 Subject: [PATCH 19/22] [gait_planner_node.py] add schmitt trigger to swich gait type --- hrpsys_ros_bridge_jvrc/config/multi_gait.rviz | 36 ++++++++-------- .../scripts/gait_planner_node.py | 42 +++++++++++++------ 2 files changed, 48 insertions(+), 30 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz index e3fbcddd..acd5de17 100644 --- a/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz +++ b/hrpsys_ros_bridge_jvrc/config/multi_gait.rviz @@ -4,7 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Grid1 - /PointCloudGroup1 + - /PointCloudGroup1/stereo1 - /PointCloudGroup1/laser1 - /select-contact-plane1 - /select-contact-plane1/PolygonArray1 @@ -34,7 +36,7 @@ Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 1 + Cell Size: 0.5 Class: rviz/Grid Color: 160; 160; 164 Enabled: true @@ -48,7 +50,7 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 100 Reference Frame: Value: true - Alpha: 1 @@ -66,9 +68,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4203 + Max Intensity: 3209 Min Color: 0; 0; 0 - Min Intensity: 72 + Min Intensity: 44 Name: LaserScan Position Transformer: XYZ Queue Size: 10 @@ -502,7 +504,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.01 + Size (m): 0.002 Style: Flat Squares Topic: /multisense_local/organized_image_points2_color Use Fixed Frame: true @@ -511,8 +513,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 6.57932 - Min Value: -0.050695 + Max Value: 0.149959 + Min Value: -0.041234 Value: true Axis: Z Channel Name: intensity @@ -533,7 +535,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.005 Style: Flat Squares - Topic: /full_cloud2 + Topic: /gait_planner_group/laser_z_filter/output Use Fixed Frame: true Use rainbow: true Value: true @@ -588,22 +590,22 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.40202 + Distance: 4.3294 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.42976 - Y: 0.598739 - Z: 1.27833 + X: 1.44266 + Y: 0.449749 + Z: 0.543117 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.0447975 + Pitch: 0.619797 Target Frame: Value: Orbit (rviz) - Yaw: 0.488554 + Yaw: 1.46173 Saved: ~ Window Geometry: Displays: @@ -611,7 +613,7 @@ Window Geometry: Height: 1388 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000003a5000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000004e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d90000003efc0100000002fb0000000800540069006d00650100000000000005d9000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005d9000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a7000004e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000028000004e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d0065010000000000000500000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000500000004e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -620,6 +622,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1497 - X: -6 + Width: 1280 + X: -7 Y: 24 diff --git a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py index 8f192d06..d35d05a8 100755 --- a/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py +++ b/hrpsys_ros_bridge_jvrc/scripts/gait_planner_node.py @@ -31,6 +31,7 @@ def __init__(self, node_name='gait_planner_node'): 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) @@ -57,26 +58,41 @@ def footstep_callback(self, msg): 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 and 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 - elif stddev[0,0] < 0.01 and gait_type == hrpsys_ros_bridge_jvrc.msg.MultiGaitGoPos.TROT: - 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 + 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) From d7a6b17c3358f0112bdbd248c380765c7b832747 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Fri, 22 Jan 2016 03:50:23 +0900 Subject: [PATCH 20/22] [gait_controller.l] decrease transition time --- hrpsys_ros_bridge_jvrc/euslisp/gait_controller.l | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 From 3684adf0cfffd28957f7870d24550ff7212bc122 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Sun, 24 Jan 2016 13:56:57 +0900 Subject: [PATCH 21/22] add heightmap.launch --- .../launch/heightmap.launch | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 hrpsys_ros_bridge_jvrc/launch/heightmap.launch diff --git a/hrpsys_ros_bridge_jvrc/launch/heightmap.launch b/hrpsys_ros_bridge_jvrc/launch/heightmap.launch new file mode 100644 index 00000000..834435b0 --- /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.3 + + + + + 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.3 + min_y: -1.0 + max_y: 1.0 + resolution_x: 800 + resolution_y: 800 + + + center_frame_id: $(arg ROBOT_FRAME) + fixed_frame_id: $(arg STATIC_FRAME) + + + target_frame_id: $(arg STATIC_FRAME) + + + + + + + + + + + + + + + + + + + + + From 58e5df6113b9b1870df3bab861bf40460b8d99a5 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Wed, 16 Mar 2016 22:22:05 +0900 Subject: [PATCH 22/22] [heightmap.launch] update parameter of heightmap --- hrpsys_ros_bridge_jvrc/launch/heightmap.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hrpsys_ros_bridge_jvrc/launch/heightmap.launch b/hrpsys_ros_bridge_jvrc/launch/heightmap.launch index 834435b0..a3ec0f20 100644 --- a/hrpsys_ros_bridge_jvrc/launch/heightmap.launch +++ b/hrpsys_ros_bridge_jvrc/launch/heightmap.launch @@ -69,7 +69,7 @@ filter_field_name: x filter_limit_min: 0.3 - filter_limit_max: 2.3 + filter_limit_max: 2.8 @@ -88,10 +88,10 @@ min_x: 0.3 - max_x: 2.3 + max_x: 2.8 min_y: -1.0 max_y: 1.0 - resolution_x: 800 + resolution_x: 1000 resolution_y: 800