def __init__(self, args):
# Initialize values
self.busy = False
self.topic_name = args.topic
self.file_name = args.file_name
self.octomap_resolution = str(args.resolution)
self.octomap_range = str(args.range)
self.octomap_frame = args.frame
# env_name = '' # TODO:
# robot_name = '' # TODO:
# obstacle = '' # TODO:
self.durations = []
# Initialize OpenRAVE environment
self.env = orpy.Environment()
self.env.SetViewer('qtcoin')
orpy.RaveSetDebugLevel(orpy.DebugLevel.Error)
# self.env.Load(env_name)
# Initialize ROS subscribers and publisher
self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array',
MarkerArray,
callback=self.process_octomap,
queue_size=1)
# Initialize octomap plugin
orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin
self.sensor_server = orpy.RaveCreateSensorSystem(self.env, "or_octomap")
self.sensor_server.SendCommand("Enable") # Enable octomap thread
self.sensor_server.SendCommand("ResetTopic " +
self.topic_name) # Reset the registered pcl topic
self.sensor_server.SendCommand("ResetResolution " +
self.octomap_resolution) # Reset the octomap resolution
self.sensor_server.SendCommand("ResetRange " +
self.octomap_range) # Reset the octomap max range
self.sensor_server.SendCommand("ResetFrameID " +
self.octomap_frame) # Reset the octomap frame
self.time_start = time.time()
def __init__(self, args):
# Initialize values
self.busy = False
self.topic_name = args.topic
rospack = rospkg.RosPack()
path = rospack.get_path('or_octomap_plugin') + '/tests/'
self.file_name = path + args.file_name
self.octomap_resolution = str(args.resolution)
self.octomap_range = args.range
self.octomap_frame = args.frame
self.timeout = args.timeout
self.pause = False
# self.env_name = '' # TODO:
# self.robot_name = '' # TODO:
# self.obstacle_name = '' # TODO:
self.durations = []
# Initialize OpenRAVE environment
self.env = orpy.Environment()
self.env.SetViewer('qtcoin')
orpy.RaveSetDebugLevel(orpy.DebugLevel.Error)
# self.env.Load(self.env_name)
# Initialize ROS subscribers and publisher
self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array',
MarkerArray,
callback=self.process_octomap,
queue_size=1)
# Initialize octomap plugin
orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin
self.sensor_server = orpy.RaveCreateSensorSystem(
self.env, "or_octomap")
self.sensor_server.SendCommand("Enable") # Enable octomap thread
self.sensor_server.SendCommand(
"ResetTopic " + self.topic_name) # Reset the registered pcl topic
self.sensor_server.SendCommand(
"ResetResolution " +
self.octomap_resolution) # Reset the octomap resolution
self.sensor_server.SendCommand(
"ResetFrameID " + self.octomap_frame) # Reset the octomap frame
# Initialize dynamic reconfigure server
self.server_d = dynamic_reconfigure.server.Server(
OctomapServerConfig, self.cb_reconfig_server, '')
# Reset octomap server parameters
self.sensor_server.SendCommand('TogglePause')
rospy.sleep(0.1)
rospy.set_param('or_octomap/sensor_model_max_range',
self.octomap_range)
self.sensor_server.SendCommand('Reset')
self.time_start = time.time()
def get_chomp_module(env):
if args.planner == "chomp":
from orcdchomp import orcdchomp
chomp_module_path = "/home/jonathan/build/chomp/liborcdchomp.so"
elif args.planner == "chomp2":
from orcdchomp2 import orcdchomp
chomp_module_path = "/home/jonathan/code/orcdchomp2/lib/liborcdchomp2.so"
else:
assert False
openravepy.RaveLoadPlugin(chomp_module_path)
m_chomp = openravepy.RaveCreateModule(env, 'orcdchomp')
env.Add(m_chomp, True, 'blah_load_string')
orcdchomp.bind(m_chomp)
return m_chomp
import atexit
import math
import numpy
import openravepy
import orcdchomp.orcdchomp
# start openrave
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
atexit.register(openravepy.RaveDestroy)
e = openravepy.Environment()
atexit.register(e.Destroy)
e.SetViewer('qtcoin')
# load the orcdchomp module
openravepy.RaveLoadPlugin('planner_plugin/build/chomp/chomp')
m_chomp = openravepy.RaveCreateModule(e, 'orcdchomp')
if not m_chomp:
raise RuntimeError('no chomp module found!')
orcdchomp.orcdchomp.bind(m_chomp)
# table
table = e.ReadKinBodyXMLFile('models/furniture/rolly-table.iv')
e.Add(table)
table.SetTransform([0.70711, 0.70711, 0, 0, 0, 0, 0])
# bottle (and its grasp)
mug = e.ReadKinBodyXMLFile('models/objects/mug3.iv')
e.Add(mug)
mug.SetTransform([1, 0, 0, 0, 0, 0, 0.7])
T_mug_palm = numpy.array([[0, -1, 0, 0.000], [0, 0, -1, 0.075],
if __name__ == "__main__":
file_name = 'octomap'
rospy.init_node('test_octomap')
rospack = rospkg.RosPack()
path = rospack.get_path('or_octomap_plugin') + '/tests/'
# Load OpenRAVE Environment
env = openravepy.Environment()
env.Load('worlds/lab_demo.env.xml')
robot = env.GetRobot('robot')
robot.SetActiveDOFValues([0, -0.34906585, 2.26892803, 0, 1.22173048, 0])
env.SetViewer('qtcoin')
# Initialize octomap
openravepy.RaveLoadPlugin("or_octomap")
sensor_server = openravepy.RaveCreateSensorSystem(env, "or_octomap")
sensor_server.SendCommand("Enable")
sensor_server.SendCommand("Load " + path + file_name)
# Create octomap and mask obstacles
rospy.loginfo('Wait for octomap to be created...')
try:
marker_array = MarkerArray()
time_start = time.time()
marker_array = rospy.wait_for_message('/occupied_cells_vis_array',
MarkerArray,
timeout=10)
t = time.time() - time_start
rospy.logwarn('Time for constructing the octomap: {}'.format(t))
sensor_server.SendCommand('TogglePause')
#!/usr/bin/python
# -*- coding: utf-8 -*-
import openravepy
import os
env = openravepy.Environment()
env.SetViewer('qtcoin')
env.Load('tutorial/env1.xml')
#robot= env.GetRobots()[0]
robot= env.GetRobot('SimpleBot')
#openravepy.RaveLoadPlugin(os.getcwd()+'/libtestcontrollers.so')
openravepy.RaveLoadPlugin(os.getcwd()+'/test_ctrl/build/libtestcontrollers.so')
controller= openravepy.RaveCreateController(env,'TestController')
robot.SetController(controller,range(robot.GetDOF()),controltransform=1)
raw_input("Press Enter to start...")
import time
env.UpdatePublishedBodies()
time.sleep(0.1)
nav = openravepy.examples.simplenavigation.SimpleNavigationPlanning(robot)
nav.performNavigationPlanning()
raw_input("Press Enter to exit...")
env.Destroy()
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#based on HW3 for RBE 595/CS 525 Motion Planning
import time
import openravepy
openravepy.RaveInitialize()
openravepy.RaveLoadPlugin('build/rrt-pluguin')
import multiprocessing, math, time
if not __openravepy_build_doc__:
from openravepy import *
from numpy import *
def waitrobot(robot):
"""busy wait for robot completion"""
while not robot.GetController().IsDone():
time.sleep(0.01)
def tuckarms(env, robot):
with env:
jointnames = [
'l_shoulder_lift_joint', 'l_elbow_flex_joint',
'l_wrist_flex_joint', 'r_shoulder_lift_joint',
'r_elbow_flex_joint', 'r_wrist_flex_joint'
]
robot.SetActiveDOFs(
[robot.GetJoint(name).GetDOFIndex() for name in jointnames])
robot.SetActiveDOFValues([