add launch/sample_walk_to_point_in_image.launch node_scripts/sample_w…

…alk_to_point_in_image.py

jsk_spot_robot/jsk_spot_startup/launch/sample_walk_to_point_in_image.launch

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="image" default="hand_color" doc="image to set object location [frontleft, frontright, left, right, hand_color]" />
+  <arg name="distance" default="0.5" doc="final distance to walk to object" />
+
+  <node pkg="image_view2" type="image_view2" name="image_view_for_pick_object" >
+    <remap from="image" to="/spot/camera/$(arg image)/image" />
+    <param name="~image_transport" value="compressed" />
+  </node>
+
+  <node pkg="jsk_spot_startup" type="sample_walk_to_point_in_image.py" name="sample_walk_to_point_in_image" output="screen" >
+    <remap from="screenpoint" to="/spot/camera/$(arg image)/image/screenpoint" />
+    <param name="~image_source" value="$(arg image)" />
+    <param name="~distance" value="$(arg distance)" />
+  </node>
+
+</launch>

jsk_spot_robot/jsk_spot_startup/node_scripts/sample_walk_to_point_in_image.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+import rospy
+import actionlib
+from geometry_msgs.msg import Point, PointStamped
+from spot_msgs.msg import WalkToObjectInImageAction, WalkToObjectInImageGoal
+
+def cb(msg):
+    rospy.loginfo('screenpoint callback x={}, y={}, distance={}'.format(msg.point.x, msg.point.y, distance))
+    timeout = 15
+    goal = WalkToObjectInImageGoal(
+        image_source = image_source,
+        center = msg.point,
+        distance = distance,
+        max_duration = rospy.Duration(timeout)
+    )
+    client.send_goal(goal)
+    client.wait_for_result(timeout=rospy.Duration(timeout+5))
+    if client.get_result().success:
+        rospy.loginfo("succeeded")
+    else:
+        rospy.logerr("failed to walkt ot object")
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('sample_walk_to_object_in_image')
+        image_source = rospy.get_param('~image_source', 'hand_color')
+        distance = rospy.get_param('~distance', 0.5)
+        # subscribe point in screen
+        rospy.loginfo('start subscribe screenpoint at {}'.format(image_source))
+        rospy.Subscriber('screenpoint', PointStamped, cb)
+        # action client for walk to point API
+        rospy.loginfo('conncting to walk_to_point_in_image action')
+        client = actionlib.SimpleActionClient('/spot/walk_to_object_in_image', WalkToObjectInImageAction)
+        client.wait_for_server()
+        #
+        rospy.loginfo('start main loop')
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        ropsy.logerr("program interrupted before completion")