mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
101 lines
2.1 KiB
Python
101 lines
2.1 KiB
Python
#! /usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
from moveit.task_constructor import core, stages
|
|
from geometry_msgs.msg import PoseStamped
|
|
import time
|
|
|
|
from py_binding_tools import roscpp_init
|
|
|
|
roscpp_init("mtc_tutorial")
|
|
|
|
# Create a task container
|
|
task = core.Task()
|
|
|
|
# [propertyTut10]
|
|
# Create a current state to capture the current planning scene state
|
|
currentState = stages.CurrentState("Current State")
|
|
# [propertyTut10]
|
|
|
|
# [propertyTut1]
|
|
# Create a property
|
|
p = core.Property()
|
|
|
|
# Set a descriptive string to describe the properties function
|
|
p.setDescription("Foo Property")
|
|
# [propertyTut1]
|
|
|
|
# Set the current and the default value
|
|
p.setValue("Bar")
|
|
|
|
# [propertyTut2]
|
|
# Check if the property is defined
|
|
assert p.defined()
|
|
# [propertyTut2]
|
|
|
|
# [propertyTut3]
|
|
# Retrieve the stored value
|
|
print(p.value())
|
|
|
|
# Retrieve the default value
|
|
print(p.defaultValue())
|
|
|
|
# Retrieve the description
|
|
print(p.description())
|
|
# [propertyTut3]
|
|
|
|
# [propertyTut4]
|
|
# Create a property map
|
|
pm = core.PropertyMap()
|
|
props = {"prop1": "test", "prop2": 21, "prop3": PoseStamped(), "prop4": 5.4}
|
|
pm.update(props)
|
|
# [propertyTut4]
|
|
|
|
# [propertyTut5]
|
|
# Add a property to the property map using the pythonic way
|
|
pm["prop5"] = 2
|
|
# [propertyTut5]
|
|
|
|
# [propertyTut6]
|
|
# Return the value of a property
|
|
print(pm["prop5"])
|
|
# [propertyTut6]
|
|
|
|
# [propertyTut7]
|
|
# Return the underlying property object
|
|
p2 = pm.property("prop5")
|
|
# [propertyTut7]
|
|
|
|
# [propertyTut8]
|
|
# Iterate through all the values in the property map
|
|
print("\n")
|
|
for i in pm:
|
|
print(i, "\t\t", pm[i])
|
|
print("\n")
|
|
# [propertyTut8]
|
|
|
|
# [propertyTut9]
|
|
# A new property map can also be configured using an existing one
|
|
# You can also only use a subset of the properties that should be configured.
|
|
pm2 = core.PropertyMap()
|
|
pm.exposeTo(pm2, ["prop2", "prop4"])
|
|
# [propertyTut9]
|
|
|
|
# Lets test that by printing out our properties
|
|
for i in pm2:
|
|
print(i, "\t\t", pm2[i])
|
|
print("\n")
|
|
|
|
# [propertyTut11]
|
|
# Access the property map of the stage
|
|
props = currentState.properties
|
|
# [propertyTut11]
|
|
|
|
# Add the stage to the task hierarchy
|
|
task.add(currentState)
|
|
|
|
if task.plan():
|
|
task.publish(task.solutions[0])
|
|
|
|
time.sleep(100)
|