(rwt_moveit) add sample obstacle.

rwt_moveit/launch/sim_demo_obstacle.launch

@@ -0,0 +1,13 @@
+<launch>
+
+<arg name="fixed_frame" default="/BASE" />
+
+<include file="$(find rwt_moveit)/launch/sim_demo.launch">
+  <arg name="fixed_frame" value="$(arg fixed_frame)" />  
+</include>
+
+<node pkg="interactive_marker_proxy" type="proxy" name="obst_marker_proxy" args="topic_ns:=/obst/marker target_frame:=$(arg fixed_frame)" />
+
+<node pkg="rwt_moveit" type="sample_obstacle_marker.py" name="node_obst_marker" />
+
+</launch>

rwt_moveit/nodes/sample_obstacle_marker.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+
+# Taken from:
+# http://answers.ros.org/question/11135/plotting-a-markerarray-of-spheres-with-rviz/?answer=16587#post-id-16587
+
+import math
+
+import rospy
+from visualization_msgs.msg import Marker
+
+topic = '/obst/marker'
+
+publisher = rospy.Publisher(topic, Marker)
+
+rospy.init_node('sample_obstacle')
+
+count = 0
+MARKERS_MAX = 2  # Up to two boxes.
+
+marker_box_1 = Marker()
+marker_box_1.header.frame_id = "/BASE"
+marker_box_1.type = marker_box_1.CUBE
+marker_box_1.action = marker_box_1.ADD
+marker_box_1.scale.x = 0.2
+marker_box_1.scale.y = 0.2
+marker_box_1.scale.z = 0.2
+marker_box_1.color.a = 1.0
+marker_box_1.color.r = 0.0
+marker_box_1.color.g = 0.3
+marker_box_1.color.b = 0.5
+marker_box_1.pose.orientation.w = 1.0
+marker_box_1.pose.position.x = 0.15
+marker_box_1.pose.position.y = 0.2
+marker_box_1.pose.position.z = 0.5
+
+marker_box_2 = Marker()
+marker_box_2.header.frame_id = "/BASE"
+marker_box_2.type = marker_box_2.CUBE
+marker_box_2.action = marker_box_2.ADD
+marker_box_2.scale.x = 0.2
+marker_box_2.scale.y = 0.2
+marker_box_2.scale.z = 0.2
+marker_box_2.color.a = 1.0
+marker_box_2.color.r = 0.0
+marker_box_2.color.g = 0.3
+marker_box_2.color.b = 0.5
+marker_box_2.pose.orientation.w = 1.0
+marker_box_2.pose.position.x = 0.15
+marker_box_2.pose.position.y = 0.2
+marker_box_2.pose.position.z = 0.15
+#    publisher.publish(marker_box_2)
+
+while not rospy.is_shutdown():
+    publisher.publish(marker_box_1)
+    rospy.sleep(0.01)
+# markerArray = MarkerArray()
+# 
+# count = 0
+# MARKERS_MAX = 100
+# 
+# while not rospy.is_shutdown():
+# 
+#    marker = Marker()
+#    marker.header.frame_id = "/BASE"
+#    marker.type = marker.SPHERE
+#    marker.action = marker.ADD
+#    marker.scale.x = 0.2
+#    marker.scale.y = 0.2
+#    marker.scale.z = 0.2
+#    marker.color.a = 1.0
+#    marker.color.r = 1.0
+#    marker.color.g = 1.0
+#    marker.color.b = 0.0
+#    marker.pose.orientation.w = 1.0
+#    marker.pose.position.x = math.cos(count / 50.0)
+#    marker.pose.position.y = math.cos(count / 40.0) 
+#    marker.pose.position.z = math.cos(count / 30.0) 
+# 
+#    # We add the new marker to the MarkerArray, removing the oldest
+#    # marker from it when necessary
+#    if(count > MARKERS_MAX):
+#        markerArray.markers.pop(0)
+# 
+#    markerArray.markers.append(marker)
+# 
+#    # Renumber the marker IDs
+#    id = 0
+#    for m in markerArray.markers:
+#        m.id = id
+#        id += 1
+# 
+#    # Publish the MarkerArray
+#    publisher.publish(markerArray)
+# 
+#    count += 1
+# 
+#    rospy.sleep(0.01)
+# 

rwt_moveit/nodes/sample_obstacle_markerarray.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+
+# Taken from:
+# http://answers.ros.org/question/11135/plotting-a-markerarray-of-spheres-with-rviz/?answer=16587#post-id-16587
+
+import math
+
+import rospy
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
+
+topic = '/obst/markerarray'
+
+publisher = rospy.Publisher(topic, MarkerArray)
+
+rospy.init_node('sample_obstacle')
+
+count = 0
+MARKERS_MAX = 2  # Up to two boxes.
+
+marker_box_1 = Marker()
+marker_box_1.header.frame_id = "/BASE"
+marker_box_1.type = marker_box_1.CUBE
+marker_box_1.action = marker_box_1.ADD
+marker_box_1.scale.x = 0.2
+marker_box_1.scale.y = 0.2
+marker_box_1.scale.z = 0.2
+marker_box_1.color.a = 1.0
+marker_box_1.color.r = 0.0
+marker_box_1.color.g = 0.3
+marker_box_1.color.b = 0.5
+marker_box_1.pose.orientation.w = 1.0
+marker_box_1.pose.position.x = 0.15
+marker_box_1.pose.position.y = 0.2
+marker_box_1.pose.position.z = 0.5
+#    publisher.publish(marker_box_1)
+
+marker_box_2 = Marker()
+marker_box_2.header.frame_id = "/BASE"
+marker_box_2.type = marker_box_2.CUBE
+marker_box_2.action = marker_box_2.ADD
+marker_box_2.scale.x = 0.2
+marker_box_2.scale.y = 0.2
+marker_box_2.scale.z = 0.2
+marker_box_2.color.a = 1.0
+marker_box_2.color.r = 0.0
+marker_box_2.color.g = 0.3
+marker_box_2.color.b = 0.5
+marker_box_2.pose.orientation.w = 1.0
+marker_box_2.pose.position.x = 0.15
+marker_box_2.pose.position.y = 0.2
+marker_box_2.pose.position.z = 0.15
+#    publisher.publish(marker_box_2)
+
+    # We add the new marker to the MarkerArray, removing the oldest
+    # marker from it when necessary
+#    if(count > MARKERS_MAX):
+#        markerArray.markers.pop(0)
+
+markerArray = MarkerArray()
+markerArray.markers.append(marker_box_1)
+markerArray.markers.append(marker_box_2)
+
+while not rospy.is_shutdown():
+    publisher.publish(markerArray)
+    rospy.sleep(0.01)
+
+# markerArray = MarkerArray()
+# 
+# count = 0
+# MARKERS_MAX = 100
+# 
+# while not rospy.is_shutdown():
+# 
+#    marker = Marker()
+#    marker.header.frame_id = "/BASE"
+#    marker.type = marker.SPHERE
+#    marker.action = marker.ADD
+#    marker.scale.x = 0.2
+#    marker.scale.y = 0.2
+#    marker.scale.z = 0.2
+#    marker.color.a = 1.0
+#    marker.color.r = 1.0
+#    marker.color.g = 1.0
+#    marker.color.b = 0.0
+#    marker.pose.orientation.w = 1.0
+#    marker.pose.position.x = math.cos(count / 50.0)
+#    marker.pose.position.y = math.cos(count / 40.0) 
+#    marker.pose.position.z = math.cos(count / 30.0) 
+# 
+#    # We add the new marker to the MarkerArray, removing the oldest
+#    # marker from it when necessary
+#    if(count > MARKERS_MAX):
+#        markerArray.markers.pop(0)
+# 
+#    markerArray.markers.append(marker)
+# 
+#    # Renumber the marker IDs
+#    id = 0
+#    for m in markerArray.markers:
+#        m.id = id
+#        id += 1
+# 
+#    # Publish the MarkerArray
+#    publisher.publish(markerArray)
+# 
+#    count += 1
+# 
+#    rospy.sleep(0.01)
+# 

rwt_moveit/www/js/basic.js

@@ -16,6 +16,8 @@ var start_im_client;
 var goal_im_client;
 var tfClient;
 
+var marker_obstacle; // trial
+
 function init() {
     // Connect to ROS.
     var url = 'ws://' + location.hostname + ':9090';
@@ -192,6 +194,15 @@ function init() {
             camera : viewer.camera,
             rootObject : viewer.selectableObjects
         });
+        marker_obstacle = new ROS3D.MarkerClient({
+          ros : real_ros,
+          tfClient : tfClient,
+          topic : '/obst/marker',
+          camera : viewer.camera,
+          //rootObject : viewer.selectableObjects
+          rootObject : viewer.scene
+    });
+
     });
 
     link_group_param.get(function(value) {