From ffa9ba0d190e4d64ff425ec524b2b27ac18e46af Mon Sep 17 00:00:00 2001 From: Isaac IY Saito <130s@lateeye.net> Date: Sat, 27 Sep 2014 11:38:09 +0900 Subject: [PATCH] (rwt_moveit) add sample obstacle. --- rwt_moveit/launch/sim_demo_obstacle.launch | 13 +++ rwt_moveit/nodes/sample_obstacle_marker.py | 98 ++++++++++++++++ .../nodes/sample_obstacle_markerarray.py | 110 ++++++++++++++++++ rwt_moveit/www/js/basic.js | 11 ++ 4 files changed, 232 insertions(+) create mode 100644 rwt_moveit/launch/sim_demo_obstacle.launch create mode 100755 rwt_moveit/nodes/sample_obstacle_marker.py create mode 100644 rwt_moveit/nodes/sample_obstacle_markerarray.py diff --git a/rwt_moveit/launch/sim_demo_obstacle.launch b/rwt_moveit/launch/sim_demo_obstacle.launch new file mode 100644 index 00000000..e4266d58 --- /dev/null +++ b/rwt_moveit/launch/sim_demo_obstacle.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/rwt_moveit/nodes/sample_obstacle_marker.py b/rwt_moveit/nodes/sample_obstacle_marker.py new file mode 100755 index 00000000..64fe40aa --- /dev/null +++ b/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) +# diff --git a/rwt_moveit/nodes/sample_obstacle_markerarray.py b/rwt_moveit/nodes/sample_obstacle_markerarray.py new file mode 100644 index 00000000..00f823d0 --- /dev/null +++ b/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) +# diff --git a/rwt_moveit/www/js/basic.js b/rwt_moveit/www/js/basic.js index 025bd41e..0f18e8d2 100644 --- a/rwt_moveit/www/js/basic.js +++ b/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) {