def _param_tag(self, tag, context, ros_config, force_local=False, verbose=True):
"""
@param force_local: if True, param must be added to context instead of ros_config
@type force_local: bool
"""
try:
self._check_attrs(tag, context, ros_config, XmlLoader.PARAM_ATTRS)
# compute name and value
ptype = (tag.getAttribute('type') or 'auto').lower().strip()
vals = self.opt_attrs(tag, context, ('value', 'textfile', 'binfile', 'command'))
if len([v for v in vals if v is not None]) != 1:
raise XmlParseException(
"<param> tag must have one and only one of value/textfile/binfile.")
# compute name. if name is a tilde name, it is placed in
# the context. otherwise it is placed in the ros config.
name = self.resolve_args(tag.attributes['name'].value.strip(), context)
value = self.param_value(verbose, name, ptype, *vals)
if is_private(name) or force_local:
p = Param(name, value)
context.add_param(p)
else:
p = Param(ns_join(context.ns, name), value)
ros_config.add_param(Param(ns_join(context.ns, name), value), filename=context.filename, verbose=verbose)
return p
except KeyError as e:
raise XmlParseException(
"<param> tag is missing required attribute: %s. \n\nParam xml is %s"%(e, tag.toxml()))
except ValueError as e:
raise XmlParseException(
"Invalid <param> tag: %s. \n\nParam xml is %s"%(e, tag.toxml()))
def _param_tag(self, tag, context, ros_config, force_local=False, verbose=True):
"""
@param force_local: if True, param must be added to context instead of ros_config
@type force_local: bool
"""
try:
self._check_attrs(tag, context, ros_config, XmlLoader.PARAM_ATTRS)
# compute name and value
ptype = (tag.getAttribute('type') or 'auto').lower().strip()
vals = self.opt_attrs(tag, context, ('value', 'textfile', 'binfile', 'command'))
if len([v for v in vals if v is not None]) != 1:
raise XmlParseException(
"<param> tag must have one and only one of value/textfile/binfile.")
# compute name. if name is a tilde name, it is placed in
# the context. otherwise it is placed in the ros config.
name = self.resolve_args(tag.attributes['name'].value.strip(), context)
value = self.param_value(verbose, name, ptype, *vals)
if is_private(name) or force_local:
p = Param(name, value)
context.add_param(p)
else:
p = Param(ns_join(context.ns, name), value)
ros_config.add_param(Param(ns_join(context.ns, name), value), filename=context.filename, verbose=verbose)
return p
except KeyError as e:
raise XmlParseException(
"<param> tag is missing required attribute: %s. \n\nParam xml is %s"%(e, tag.toxml()))
except ValueError as e:
raise XmlParseException(
"Invalid <param> tag: %s. \n\nParam xml is %s"%(e, tag.toxml()))
def __init__(self, robot: DualArmRobot, controller_name: str):
self.robot = robot
self.action_name = ns_join(robot.robot_namespace, ns_join(controller_name, "follow_joint_trajectory"))
self.server = actionlib.SimpleActionServer(self.action_name, FollowJointTrajectoryAction,
execute_cb=self.follow_trajectory_cb,
auto_start=False)
self.client = actionlib.SimpleActionClient(self.action_name, FollowJointTrajectoryAction)
def print_params(params, ns):
"""
Print contents of param dictionary to screen
"""
if type(params) == dict:
for k, v in params.items():
if type(v) == dict:
print_params(v, ns_join(ns, k))
else:
print("%s=%s"%(ns_join(ns, k), v))
else:
print(params)
def print_params(params, ns):
"""
Print contents of param dictionary to screen
"""
if type(params) == dict:
for k, v in params.items():
if type(v) == dict:
print_params(v, ns_join(ns, k))
else:
print("%s=%s" % (ns_join(ns, k), v))
else:
print(params)
def _set_param(self, ctx, my_state, test_vals, param_server):
ctx = make_global_ns(ctx)
for type, vals in test_vals:
try:
caller_id = ns_join(ctx, "node")
count = 0
for val in vals:
key = ns_join(caller_id, "%s-%s"%(type,count))
param_server.set_param(key, val)
self.assert_(param_server.has_param(key))
true_key = ns_join(ctx, key)
my_state[true_key] = val
count += 1
except Exception:
assert "getParam failed on type[%s], val[%s]"%(type,val)
def __init__(self, ns='', synchronous=False, service_timeout=5.0):
self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns)
self._pub_co = rospy.Publisher(ns_join(ns, 'collision_object'),
CollisionObject,
queue_size=100)
self._pub_aco = rospy.Publisher(ns_join(ns,
'attached_collision_object'),
AttachedCollisionObject,
queue_size=100)
self.__synchronous = synchronous
if self.__synchronous:
self._apply_planning_scene_diff = rospy.ServiceProxy(
ns_join(ns, 'apply_planning_scene'), ApplyPlanningScene)
self._apply_planning_scene_diff.wait_for_service(service_timeout)
def resolve_name(name, caller_id=None):
"""
Resolve a ROS name to its global, canonical form. Private ~names
are resolved relative to the node name.
@param name: name to resolve.
@type name: str
@param caller_id: node name to resolve relative to. To
resolve to local namespace, omit this parameter (or use None)
@type caller_id: str
@return: Resolved name. If name is empty/None, resolve_name
returns parent namespace. If namespace is empty/None,
@rtype: str
"""
if not caller_id:
caller_id = get_name()
if not name: #empty string resolves to namespace
return namespace(caller_id)
name = str(name) # enforce string conversion else struct.pack might raise UnicodeDecodeError (see #3998)
name = canonicalize_name(name)
if name[0] == SEP: #global name
resolved_name = name
elif is_private(name): #~name
resolved_name = ns_join(caller_id, name[1:])
else: #relative
resolved_name = namespace(caller_id) + name
#Mappings override general namespace-based resolution
# - do this before canonicalization as remappings are meant to
# match the name as specified in the code
if resolved_name in _resolved_mappings:
return _resolved_mappings[resolved_name]
else:
return resolved_name
def resolve_name_without_node_name(name):
"""
The need for this function is complicated -- Topics and Services can be created before init_node is called.
In general, this is okay, unless the name is a ~name, in which
case we have to raise an ValueError
@param name: ROS name to resolve
@type name: str
@raise ValueError: if name is a ~name
@raise ROSInitException: if name is remapped to a ~name
"""
if is_private(name):
raise ValueError("~name topics cannot be created before init_node() has been called")
# we use the underlying rosgraph.names.resolve_name to avoid dependencies on nodename/remappings
fake_caller_id = ns_join(get_namespace(), 'node')
fake_resolved = rosgraph.names.resolve_name(name, fake_caller_id)
for m, v in _mappings.items():
if rosgraph.names.resolve_name(m, fake_caller_id) == fake_resolved:
if is_private(name):
raise ROSInitException("due to the way this node is written, %s cannot be remapped to a ~name. \nThe declaration of topics/services must be moved after the call to init_node()"%name)
else:
return rosgraph.names.resolve_name(v, fake_caller_id)
return fake_resolved
def set_param_raw(param, value, verbose=False):
"""
Set param on the Parameter Server. Unlike L{set_param()}, this
takes in a Python value to set instead of YAML.
:param param: parameter name, ``str``
:param value XmlRpcLegalValue: value to upload, ``XmlRpcLegalValue``
"""
if type(value) == dict:
# #1098 changing dictionary behavior to be an update, rather
# than replace behavior.
for k, v in value.items():
# dictionary keys must be non-unicode strings
if isinstance(k, str):
set_param_raw(ns_join(param, k), v, verbose=verbose)
else:
raise RosParamException("YAML dictionaries must have string keys. Invalid dictionary is:\n%s"%value)
else:
try:
expected_type = long
except NameError :
expected_type = int
if type(value) == expected_type:
if value > sys.maxsize:
raise RosParamException("Overflow: Parameter Server integers must be 32-bit signed integers:\n\t-%s <= value <= %s"%(maxint - 1, maxint))
try:
get_param_server().setParam(param, value)
except socket.error:
raise RosParamIOException("Unable to communicate with master!")
if verbose:
print("set parameter [%s] to [%s]"%(param, value))
def __init__(self, ns='', synchronous=False, service_timeout=5.0):
""" Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """
self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns)
self._pub_co = rospy.Publisher(ns_join(ns, 'collision_object'),
CollisionObject,
queue_size=100)
self._pub_aco = rospy.Publisher(ns_join(ns,
'attached_collision_object'),
AttachedCollisionObject,
queue_size=100)
self.__synchronous = synchronous
if self.__synchronous:
self._apply_planning_scene_diff = rospy.ServiceProxy(
ns_join(ns, 'apply_planning_scene'), ApplyPlanningScene)
self._apply_planning_scene_diff.wait_for_service(service_timeout)
def set_param_raw(param, value, verbose=False):
"""
Set param on the Parameter Server. Unlike L{set_param()}, this
takes in a Python value to set instead of YAML.
:param param: parameter name, ``str``
:param value XmlRpcLegalValue: value to upload, ``XmlRpcLegalValue``
"""
if type(value) == dict:
# #1098 changing dictionary behavior to be an update, rather
# than replace behavior.
for k, v in value.items():
# dictionary keys must be non-unicode strings
if isinstance(k, str):
set_param_raw(ns_join(param, k), v, verbose=verbose)
else:
raise RosParamException("YAML dictionaries must have string keys. Invalid dictionary is:\n%s"%value)
else:
try:
expected_type = long
except NameError :
expected_type = int
if type(value) == expected_type:
if value > sys.maxsize:
raise RosParamException("Overflow: Parameter Server integers must be 32-bit signed integers:\n\t-%s <= value <= %s"%(maxint - 1, maxint))
try:
get_param_server().setParam(param, value)
except socket.error:
raise RosParamIOException("Unable to communicate with master!")
if verbose:
print("set parameter [%s] to [%s]"%(param, value))
def process(self, loader, ros_launch_config):
if self.namespace is None:
param = '/'
else:
param = rn.ns_join('', self.namespace)
loader.load_rosparam(loader.root_context, ros_launch_config,
self.command, param, self.param_file, '')
def resolve_name(name, caller_id=None):
"""
Resolve a ROS name to its global, canonical form. Private ~names
are resolved relative to the node name.
@param name: name to resolve.
@type name: str
@param caller_id: node name to resolve relative to. To
resolve to local namespace, omit this parameter (or use None)
@type caller_id: str
@return: Resolved name. If name is empty/None, resolve_name
returns parent namespace. If namespace is empty/None,
@rtype: str
"""
if not caller_id:
caller_id = get_name()
if not name: #empty string resolves to namespace
return namespace(caller_id)
name = canonicalize_name(name)
if name[0] == SEP: #global name
resolved_name = name
elif is_private(name): #~name
resolved_name = ns_join(caller_id, name[1:])
else: #relative
resolved_name = namespace(caller_id) + name
#Mappings override general namespace-based resolution
# - do this before canonicalization as remappings are meant to
# match the name as specified in the code
if resolved_name in _resolved_mappings:
return _resolved_mappings[resolved_name]
else:
return resolved_name
def child(self, ns):
"""
@param ns: sub-namespace of child context, or None if the
child context shares the same namespace
@type ns: str
@return: A child xml context that inherits from this context
@rtype: L{LoaderContext}
"""
if ns:
if ns[0] == '/': # global (discouraged)
child_ns = ns
elif ns == PRIV_NAME: # ~name
# private names can only be scoped privately or globally
child_ns = PRIV_NAME
else:
child_ns = ns_join(self.ns, ns)
else:
child_ns = self.ns
return LoaderContext(child_ns,
self.filename,
parent=self,
params=self.params,
env_args=self.env_args[:],
resolve_dict=deepcopy(self.resolve_dict),
arg_names=self.arg_names[:],
include_resolve_dict=self.include_resolve_dict)
def add_param(self, ros_config, param_name, param_value, verbose=True):
"""
Add L{Param} instances to launch config. Dictionary values are
unrolled into individual parameters.
@param ros_config: launch configuration
@type ros_config: L{ROSLaunchConfig}
@param param_name: name of parameter namespace to load values
into. If param_name is '/', param_value must be a dictionary
@type param_name: str
@param param_value: value to assign to param_name. If
param_value is a dictionary, it's values will be unrolled
into individual parameters.
@type param_value: str
@raise ValueError: if parameters cannot be processed into valid Params
"""
# shouldn't ever happen
if not param_name:
raise ValueError("no parameter name specified")
if param_name == '/' and type(param_value) != dict:
raise ValueError(
"Cannot load non-dictionary types into global namespace '/'")
if type(param_value) == dict:
# unroll params
for k, v in param_value.items():
self.add_param(ros_config,
ns_join(param_name, k),
v,
verbose=verbose)
else:
ros_config.add_param(Param(param_name, param_value),
verbose=verbose)
def __init__(self, robot_namespace: str = ''):
"""
This class is designed around the needs of the trajectory_follower.TrajectoryFollower
This class really only contains API that is needed by that class. The trajectory follower only needs to know about the
basic ROS API -- how to send and get joint commands. However, because MoveIt trajectory execution relies (for now) on that
TrajectoryFollower class, we do not want BaseRobot to use any MoveIt TrajectoryExeuction or rely on any trajectory
execution services.
"""
self.robot_namespace = robot_namespace
# the robot namespace will be prepended by setting ROS_NAMESPACE environment variable or the ns="" in roslaunch
joint_states_topic = ns_join(self.robot_namespace, 'joint_states')
self.joint_state_listener = Listener(joint_states_topic, JointState)
# NOTE: derived classes must set these values
self.right_gripper_command_pub = None
self.left_gripper_command_pub = None
self.tf_wrapper = TF2Wrapper()
try:
self.robot_commander = moveit_commander.RobotCommander(
ns=self.robot_namespace)
except RuntimeError as e:
rospy.logerr(
"You may need to load the moveit planning context and robot description"
)
print(e)
def resolve_name_without_node_name(name):
"""
The need for this function is complicated -- Topics and Services can be created before init_node is called.
In general, this is okay, unless the name is a ~name, in which
case we have to raise an ValueError
@param name: ROS name to resolve
@type name: str
@raise ValueError: if name is a ~name
@raise ROSInitException: if name is remapped to a ~name
"""
if is_private(name):
raise ValueError("~name topics cannot be created before init_node() has been called")
# we use the underlying rosgraph.names.resolve_name to avoid dependencies on nodename/remappings
fake_caller_id = ns_join(get_namespace(), 'node')
fake_resolved = rosgraph.names.resolve_name(name, fake_caller_id)
for m, v in _mappings.items():
if rosgraph.names.resolve_name(m, fake_caller_id) == fake_resolved:
if is_private(name):
raise ROSInitException("due to the way this node is written, %s cannot be remapped to a ~name. \nThe declaration of topics/services must be moved after the call to init_node()"%name)
else:
return rosgraph.names.resolve_name(v, fake_caller_id)
return fake_resolved
def _rosparam_tag(self, tag, context, ros_config, verbose=True):
try:
self._check_attrs(tag, context, ros_config,
XmlLoader.ROSPARAM_OPT_ATTRS)
cmd, ns, file, param, subst_value = self.opt_attrs(
tag, context, (XmlLoader.ROSPARAM_OPT_ATTRS))
subst_value = _bool_attr(subst_value, False, 'subst_value')
# ns atribute is a bit out-moded and is only left in for backwards compatibility
param = ns_join(ns or '', param or '')
# load is the default command
cmd = cmd or 'load'
value = _get_text(tag)
if subst_value:
value = self.resolve_args(value, context)
self.load_rosparam(context,
ros_config,
cmd,
param,
file,
value,
verbose=verbose)
except ValueError as e:
raise loader.LoadException("error loading <rosparam> tag: \n\t" +
str(e) + "\nXML is %s" % tag.toxml())
def add_param(self, ros_config, param_name, param_value, verbose=True):
"""
Add L{Param} instances to launch config. Dictionary values are
unrolled into individual parameters.
@param ros_config: launch configuration
@type ros_config: L{ROSLaunchConfig}
@param param_name: name of parameter namespace to load values
into. If param_name is '/', param_value must be a dictionary
@type param_name: str
@param param_value: value to assign to param_name. If
param_value is a dictionary, it's values will be unrolled
into individual parameters.
@type param_value: str
@raise ValueError: if parameters cannot be processed into valid Params
"""
# shouldn't ever happen
if not param_name:
raise ValueError("no parameter name specified")
if param_name == '/' and type(param_value) != dict:
raise ValueError("Cannot load non-dictionary types into global namespace '/'")
if type(param_value) == dict:
# unroll params
for k, v in param_value.items():
self.add_param(ros_config, ns_join(param_name, k), v, verbose=verbose)
else:
ros_config.add_param(Param(param_name, param_value), verbose=verbose)
def __init__(self):
super().__init__('victor')
self.service_provider = GazeboServices()
# register a new callback to stop when the rope is overstretched
self.set_rope_end_points_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "set"), Position3DAction)
def _get_param_names(names, key, d):
"""
helper recursive routine for getParamNames()
@param names: list of param names to append to
@type names: [str]
@param d: parameter tree node
@type d: dict
@param key: parameter key for tree node d
@type key: str
"""
#TODOXXX
for k,v in d.iteritems():
if type(v) == dict:
_get_param_names(names, ns_join(key, k), v)
else:
names.append(ns_join(key, k))
def __init__(self):
super().__init__()
self.color_image_listener = Listener(self.COLOR_IMAGE_TOPIC, Image)
self.depth_image_listener = Listener(self.DEPTH_IMAGE_TOPIC, Image)
self.state_color_viz_pub = rospy.Publisher("state_color_viz",
Image,
queue_size=10,
latch=True)
self.state_depth_viz_pub = rospy.Publisher("state_depth_viz",
Image,
queue_size=10,
latch=True)
self.last_action = None
self.get_rope_end_points_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "get_dual_gripper_points"),
GetDualGripperPoints)
self.get_rope_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "get_rope_state"), GetRopeState)
self.pos3d = Position3D()
self.cdcpd_listener = Listener("cdcpd/output", PointCloud2)
self.set_rope_state_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "set_rope_state"), SetRopeState)
self.reset_srv = rospy.ServiceProxy("/gazebo/reset_simulation", Empty)
try:
self.left_gripper_bbox_pub = rospy.Publisher(
'/left_gripper_bbox_pub',
BoundingBox,
queue_size=10,
latch=True)
self.right_gripper_bbox_pub = rospy.Publisher(
'/right_gripper_bbox_pub',
BoundingBox,
queue_size=10,
latch=True)
except NameError:
pass
self.overstretching_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "rope_overstretched"),
GetOverstretching)
self.error_pub = rospy.Publisher("error", Float32, queue_size=10)
self.max_action_attempts = 100
self.robot_reset_rng = np.random.RandomState(0)
def test_script_resolve_name():
from rosgraph.names import script_resolve_name, get_ros_namespace, ns_join
assert '/global' == script_resolve_name('/myscript', '/global')
val = script_resolve_name('/myscript', '')
assert get_ros_namespace() == val, val
val = script_resolve_name('/myscript', 'foo')
assert ns_join(get_ros_namespace(), 'foo') == val, val
assert '/myscript/private' == script_resolve_name('/myscript', '~private')
def test_script_resolve_name():
from rosgraph.names import script_resolve_name, get_ros_namespace, ns_join
assert '/global' == script_resolve_name('/myscript', '/global')
val = script_resolve_name('/myscript', '')
assert get_ros_namespace() == val, val
val = script_resolve_name('/myscript', 'foo')
assert ns_join(get_ros_namespace(), 'foo') == val, val
assert '/myscript/private' == script_resolve_name('/myscript', '~private')
def __init__(self):
super().__init__()
self.service_provider = BaseServices()
self.pos3d = Position3D()
self.get_rope_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "get_rope_state"), GetRopeState)
self.reset_sim_srv = rospy.ServiceProxy("/gazebo/reset_simulation",
Empty)
self.last_action = None
self.max_action_attempts = 1000
def _ns_clear_params_attr(self,
tag_name,
tag,
context,
ros_config,
node_name=None,
include_filename=None):
"""
Common processing routine for xml tags with NS and CLEAR_PARAMS attributes
@param tag: DOM Node
@type tag: Node
@param context: current namespace context
@type context: LoaderContext
@param clear_params: list of params to clear
@type clear_params: [str]
@param node_name: name of node (for use when tag_name == 'node')
@type node_name: str
@param include_filename: <include> filename if this is an <include> tag. If specified, context will use include rules.
@type include_filename: str
@return: loader context
@rtype: L{LoaderContext}
"""
if tag.hasAttribute(NS):
ns = self.resolve_args(tag.getAttribute(NS), context)
if not ns:
raise XmlParseException(
"<%s> tag has an empty '%s' attribute" % (tag_name, NS))
else:
ns = None
if include_filename is not None:
child_ns = context.include_child(ns, include_filename)
else:
child_ns = context.child(ns)
clear_p = self.resolve_args(tag.getAttribute(CLEAR_PARAMS), context)
if clear_p:
clear_p = _bool_attr(clear_p, False, 'clear_params')
if clear_p:
if tag_name == 'node':
if not node_name:
raise XmlParseException(
"<%s> tag must have a 'name' attribute to use '%s' attribute"
% (tag_name, CLEAR_PARAMS))
# use make_global_ns to give trailing slash in order to be consistent with XmlContext.ns
ros_config.add_clear_param(
make_global_ns(ns_join(child_ns.ns, node_name)))
else:
if not ns:
raise XmlParseException(
"'ns' attribute must be set in order to use 'clear_params'"
)
ros_config.add_clear_param(child_ns.ns)
return child_ns
def __init__(self, robot_namespace):
super().__init__()
self.robot_namespace = robot_namespace
self.service_provider = BaseServices()
self.joint_state_viz_pub = rospy.Publisher(ns_join(
self.robot_namespace, "joint_states_viz"),
JointState,
queue_size=10)
self.goto_home_srv = rospy.ServiceProxy("goto_home", Empty)
self.cdcpd_listener = Listener("cdcpd/output", PointCloud2)
self.cdcpd_reset_srv = rospy.ServiceProxy("cdcpd/reset", Empty)
self.attach_srv = rospy.ServiceProxy("/link_attacher_node/attach",
Attach)
self.detach_srv = rospy.ServiceProxy("/link_attacher_node/detach",
Attach)
exclude_srv_name = ns_join(self.robot_namespace,
"exclude_models_from_planning_scene")
self.exclude_from_planning_scene_srv = rospy.ServiceProxy(
exclude_srv_name, ExcludeModels)
# FIXME: this blocks until the robot is available, we need lazy construction
self.robot = get_moveit_robot(self.robot_namespace)
def _rosparam_tag(self, tag, context, ros_config, verbose=True):
try:
self._check_attrs(tag, context, ros_config, XmlLoader.ROSPARAM_OPT_ATTRS)
cmd, ns, file, param = self.opt_attrs(tag, context, (XmlLoader.ROSPARAM_OPT_ATTRS))
# ns atribute is a bit out-moded and is only left in for backwards compatibility
param = ns_join(ns or '', param or '')
# load is the default command
cmd = cmd or 'load'
self.load_rosparam(context, ros_config, cmd, param, file, _get_text(tag), verbose=verbose)
except ValueError as e:
raise loader.LoadException("error loading <rosparam> tag: \n\t"+str(e)+"\nXML is %s"%tag.toxml())
def _compute_all_keys(param_key, param_value, all_keys=None):
"""
Compute which subscribers should be notified based on the parameter update
@param param_key: key of updated parameter
@type param_key: str
@param param_value: value of updated parameter
@param all_keys: (internal use only) list of parameter keys
to append to for recursive calls.
@type all_keys: [str]
@return: list of parameter keys. All keys will be canonicalized with trailing slash.
@rtype: [str]
"""
if all_keys is None:
all_keys = []
for k, v in param_value.iteritems():
new_k = ns_join(param_key, k) + SEP
all_keys.append(new_k)
if type(v) == dict:
_compute_all_keys(new_k, v, all_keys)
return all_keys
def _ns_clear_params_attr(self, tag_name, tag, context, ros_config, node_name=None, include_filename=None):
"""
Common processing routine for xml tags with NS and CLEAR_PARAMS attributes
@param tag: DOM Node
@type tag: Node
@param context: current namespace context
@type context: LoaderContext
@param clear_params: list of params to clear
@type clear_params: [str]
@param node_name: name of node (for use when tag_name == 'node')
@type node_name: str
@param include_filename: <include> filename if this is an <include> tag. If specified, context will use include rules.
@type include_filename: str
@return: loader context
@rtype: L{LoaderContext}
"""
if tag.hasAttribute(NS):
ns = self.resolve_args(tag.getAttribute(NS), context)
if not ns:
raise XmlParseException("<%s> tag has an empty '%s' attribute"%(tag_name, NS))
else:
ns = None
if include_filename is not None:
child_ns = context.include_child(ns, include_filename)
else:
child_ns = context.child(ns)
clear_p = self.resolve_args(tag.getAttribute(CLEAR_PARAMS), context)
if clear_p:
clear_p = _bool_attr(clear_p, False, 'clear_params')
if clear_p:
if tag_name == 'node':
if not node_name:
raise XmlParseException("<%s> tag must have a 'name' attribute to use '%s' attribute"%(tag_name, CLEAR_PARAMS))
# use make_global_ns to give trailing slash in order to be consistent with XmlContext.ns
ros_config.add_clear_param(make_global_ns(ns_join(child_ns.ns, node_name)))
else:
if not ns:
raise XmlParseException("'ns' attribute must be set in order to use 'clear_params'")
ros_config.add_clear_param(child_ns.ns)
return child_ns
def load_str(str, filename, default_namespace=None, verbose=False):
"""
Load the YAML document as a string
:param filename: name of filename, only used for debugging, ``str``
:param default_namespace: namespace to load filename into, ``str``
:param str: YAML text, ``str``
:returns: list of parameter dictionary and
corresponding namespaces for each YAML document in the file, ``[(dict, str)...]``
"""
paramlist = []
default_namespace = default_namespace or get_ros_namespace()
for doc in yaml.load_all(str):
if NS in doc:
ns = ns_join(default_namespace, doc.get(NS, None))
if verbose:
print("reading parameters into namespace [%s]"%ns)
del doc[NS]
else:
ns = default_namespace
paramlist.append((doc, ns))
return paramlist
def load_str(str, filename, default_namespace=None, verbose=False):
"""
Load the YAML document as a string
:param filename: name of filename, only used for debugging, ``str``
:param default_namespace: namespace to load filename into, ``str``
:param str: YAML text, ``str``
:returns: list of parameter dictionary and
corresponding namespaces for each YAML document in the file, ``[(dict, str)...]``
"""
paramlist = []
default_namespace = default_namespace or get_ros_namespace()
for doc in yaml.load_all(str):
if NS in doc:
ns = ns_join(default_namespace, doc.get(NS, None))
if verbose:
print("reading parameters into namespace [%s]" % ns)
del doc[NS]
else:
ns = default_namespace
paramlist.append((doc, ns))
return paramlist
def child(self, ns):
"""
@param ns: sub-namespace of child context, or None if the
child context shares the same namespace
@type ns: str
@return: A child xml context that inherits from this context
@rtype: L{LoaderContext}
"""
if ns:
if ns[0] == '/': # global (discouraged)
child_ns = ns
elif ns == PRIV_NAME: # ~name
# private names can only be scoped privately or globally
child_ns = PRIV_NAME
else:
child_ns = ns_join(self.ns, ns)
else:
child_ns = self.ns
return LoaderContext(child_ns, self.filename, parent=self,
params=self.params, env_args=self.env_args[:],
resolve_dict=deepcopy(self.resolve_dict),
arg_names=self.arg_names[:], include_resolve_dict=self.include_resolve_dict)
def process(self, loader, ros_launch_config):
context = loader.root_context
# Add all our params to the ROSLaunchConfig
param_ns = context.child(self.node_name)
for name, value in self.params.iteritems():
loader_value = loader.param_value(self.verbose, name, 'auto',
py_types_to_string(value), None,
None, None)
p = rr.Param(param_ns.ns + name, loader_value)
ros_launch_config.add_param(p, verbose=self.verbose)
for rp in self.rosparams:
if rp.get_namespace() is None:
rp.set_namespace(param_ns.ns)
else:
rp.set_namespace(rn.ns_join(param_ns.ns, rp.get_namespace()))
rp.process(loader, ros_launch_config)
# Add to a LoaderContext verify that names are legal
remap_ns = context.child('')
for r in self.remaps:
remap_ns.add_remap(r)
# Create our node
self.node = rr.Node(
self.package_name,
self.node_type,
self.node_name,
remap_args=remap_ns.remap_args(),
# Setting this pipes mutes the node's stdout.
# namespace=param_ns.ns,
namespace=self.namespace,
args=self.args,
respawn=self.respawn,
respawn_delay=self.respawn_delay,
output=self.output,
launch_prefix=self.launch_prefix)
ros_launch_config.add_node(self.node, self.verbose)
def load_rosparam(self, context, ros_config, cmd, param, file_, text, verbose=True):
"""
Load rosparam setting
@param context: Loader context
@type context: L{LoaderContext}
@param ros_config: launch configuration
@type ros_config: L{ROSLaunchConfig}
@param cmd: 'load', 'dump', or 'delete'
@type cmd: str
@param file_: filename for rosparam to use or None
@type file_: str
@param text: text for rosparam to load. Ignored if file_ is set.
@type text: str
@raise ValueError: if parameters cannot be processed into valid rosparam setting
"""
if not cmd in ('load', 'dump', 'delete'):
raise ValueError("command must be 'load', 'dump', or 'delete'")
if file_ is not None:
if cmd == 'load' and not os.path.isfile(file_):
raise ValueError("file does not exist [%s]"%file_)
if cmd == 'delete':
raise ValueError("'file' attribute is invalid with 'delete' command.")
full_param = ns_join(context.ns, param) if param else context.ns
if cmd == 'dump':
ros_config.add_executable(RosbinExecutable('rosparam', (cmd, file_, full_param), PHASE_SETUP))
elif cmd == 'delete':
ros_config.add_executable(RosbinExecutable('rosparam', (cmd, full_param), PHASE_SETUP))
elif cmd == 'load':
# load YAML text
if file_:
with open(file_, 'r') as f:
text = f.read()
# parse YAML text
# - lazy import
global yaml
if yaml is None:
import yaml
# - lazy import: we have to import rosparam in oder to to configure the YAML constructors
global rosparam
if rosparam is None:
import rosparam
try:
data = yaml.load(text)
# #3162: if there is no YAML, load() will return an
# empty string. We want an empty dictionary instead
# for our representation of empty.
if data is None:
data = {}
except yaml.MarkedYAMLError as e:
if not file_:
raise ValueError("Error within YAML block:\n\t%s\n\nYAML is:\n%s"%(str(e), text))
else:
raise ValueError("file %s contains invalid YAML:\n%s"%(file_, str(e)))
except Exception as e:
if not file_:
raise ValueError("invalid YAML: %s\n\nYAML is:\n%s"%(str(e), text))
else:
raise ValueError("file %s contains invalid YAML:\n%s"%(file_, str(e)))
# 'param' attribute is required for non-dictionary types
if not param and type(data) != dict:
raise ValueError("'param' attribute must be set for non-dictionary values")
self.add_param(ros_config, full_param, data, verbose=verbose)
else:
raise ValueError("unknown command %s"%cmd)
def search_param(self, ns, key):
"""
Search for matching parameter key for search param
key. Search for key starts at ns and proceeds upwards to
the root. As such, search_param should only be called with a
relative parameter name.
search_param's behavior is to search for the first partial match.
For example, imagine that there are two 'robot_description' parameters:
- /robot_description
- /robot_description/arm
- /robot_description/base
- /pr2/robot_description
- /pr2/robot_description/base
If I start in the namespace /pr2/foo and search for
'robot_description', search_param will match
/pr2/robot_description. If I search for 'robot_description/arm'
it will return /pr2/robot_description/arm, even though that
parameter does not exist (yet).
@param ns: namespace to begin search from.
@type ns: str
@param key: Parameter key.
@type key: str
@return: key of matching parameter or None if no matching
parameter.
@rtype: str
"""
if not key or is_private(key):
raise ValueError("invalid key")
if not is_global(ns):
raise ValueError("namespace must be global")
if is_global(key):
if self.has_param(key):
return key
else:
return None
# there are more efficient implementations, but our hiearchy
# is not very deep and this is fairly clean code to read.
# - we only search for the first namespace in the key to check for a match
key_namespaces = [x for x in key.split(SEP) if x]
key_ns = key_namespaces[0]
# - corner case: have to test initial namespace first as
# negative indices won't work with 0
search_key = ns_join(ns, key_ns)
if self.has_param(search_key):
# resolve to full key
return ns_join(ns, key)
namespaces = [x for x in ns.split(SEP) if x]
for i in xrange(1, len(namespaces)+1):
search_key = SEP + SEP.join(namespaces[0:-i] + [key_ns])
if self.has_param(search_key):
# we have a match on the namespace of the key, so
# compose the full key and return it
full_key = SEP + SEP.join(namespaces[0:-i] + [key])
return full_key
return None
# aco.link_name = link
# if len(touch_links) > 0:
# aco.touch_links = touch_links
# else:
# aco.touch_links = [link]
# self.__submit(aco, attach=True)
if __name__ == "__main__":
if len(sys.argv) < 3:
print("Two argument required:")
print("<setup_name> <work_name>:\tpublish urdf file <work_name>.urdf.")
exit()
else:
package_name = sys.argv[1] + "_support"
work_name = sys.argv[2]
rospy.init_node("publish_work")
aco_pub = rospy.Publisher(ns_join('', 'attached_collision_object'),
AttachedCollisionObject,
queue_size=100)
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(1.0) # wait for the above things to setup
remove_all_objects(scene)
work = parse_urdf_file(package_name, work_name)
publish_parsed_urdf(work, scene, aco_pub, attach=False)
print("Done!")
def ns(self, name: str):
return ns_join(self.robot_namespace, name)
def get_camera_params(camera_name: str):
camera_params_topic_name = ns_join(ns_join(camera_name, 'qhd'),
"camera_info")
camera_params_listener = Listener(camera_params_topic_name, CameraInfo)
camera_params: CameraInfo = camera_params_listener.get()
return camera_params
class FloatingRopeScenario(Base3DScenario):
IMAGE_H = 90
IMAGE_W = 160
n_links = 25
KINECT_NAME = "kinect2"
COLOR_IMAGE_TOPIC = ns_join(KINECT_NAME, "qhd/image_color_rect")
DEPTH_IMAGE_TOPIC = ns_join(KINECT_NAME, "qhd/image_depth_rect")
crop_region = {
'min_y': 0,
'min_x': 0,
'max_y': 540,
'max_x': 960,
}
ROPE_NAMESPACE = 'rope_3d'
# TODO: break out the different pieces of get_state to make them composable,
# since there are just a few shared amongst all the scenarios
# TODO: about this... maybe they should all be pure functions? do we really need "self" at all?
# the one reason we have classes at all is so that we can describe interfaces via type hints
def __init__(self):
super().__init__()
self.color_image_listener = Listener(self.COLOR_IMAGE_TOPIC, Image)
self.depth_image_listener = Listener(self.DEPTH_IMAGE_TOPIC, Image)
self.state_color_viz_pub = rospy.Publisher("state_color_viz",
Image,
queue_size=10,
latch=True)
self.state_depth_viz_pub = rospy.Publisher("state_depth_viz",
Image,
queue_size=10,
latch=True)
self.last_action = None
self.get_rope_end_points_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "get_dual_gripper_points"),
GetDualGripperPoints)
self.get_rope_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "get_rope_state"), GetRopeState)
self.pos3d = Position3D()
self.cdcpd_listener = Listener("cdcpd/output", PointCloud2)
self.set_rope_state_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "set_rope_state"), SetRopeState)
self.reset_srv = rospy.ServiceProxy("/gazebo/reset_simulation", Empty)
try:
self.left_gripper_bbox_pub = rospy.Publisher(
'/left_gripper_bbox_pub',
BoundingBox,
queue_size=10,
latch=True)
self.right_gripper_bbox_pub = rospy.Publisher(
'/right_gripper_bbox_pub',
BoundingBox,
queue_size=10,
latch=True)
except NameError:
pass
self.overstretching_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "rope_overstretched"),
GetOverstretching)
self.error_pub = rospy.Publisher("error", Float32, queue_size=10)
self.max_action_attempts = 100
self.robot_reset_rng = np.random.RandomState(0)
def needs_reset(self):
res: GetOverstretchingResponse = self.overstretching_srv(
GetOverstretchingRequest())
return res.magnitude > 1.30
def trajopt_distance_to_goal_differentiable(self, final_state,
goal_state: Dict):
return self.cfm_distance(final_state['z'], goal_state['z'])
def trajopt_distance_differentiable(self, s1, s2):
return self.cfm_distance(s1['z'], s2['z'])
def cfm_distance(self, z1, z2):
return tf.math.reduce_sum(tf.math.square(z1 - z2),
axis=-1,
keepdims=True)
def get_environment(self, params: Dict, **kwargs):
return {}
def hard_reset(self):
self.reset_srv(EmptyRequest())
def randomization_initialization(self):
pass
def on_before_action(self):
self.register_fake_grasping()
def on_before_data_collection(self, params: Dict):
self.on_before_action()
left_gripper_position = np.array([1.0, 0.2, 1.0])
right_gripper_position = np.array([1.0, -0.2, 1.0])
init_action = {
'left_gripper_position': left_gripper_position,
'right_gripper_position': right_gripper_position,
}
self.execute_action(init_action)
def execute_action(self, action: Dict):
speed_mps = action.get('speed', 0.1)
left_req = Position3DActionRequest(
speed_mps=speed_mps,
scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'left_gripper'),
position=ros_numpy.msgify(Point, action['left_gripper_position']))
right_req = Position3DActionRequest(
speed_mps=speed_mps,
scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'right_gripper'),
position=ros_numpy.msgify(Point, action['right_gripper_position']))
self.pos3d.set(left_req)
self.pos3d.set(right_req)
wait_req = Position3DWaitRequest()
wait_req.timeout_s = 10.0
wait_req.scoped_link_names.append(
gz_scope(self.ROPE_NAMESPACE, 'left_gripper'))
wait_req.scoped_link_names.append(
gz_scope(self.ROPE_NAMESPACE, 'right_gripper'))
self.pos3d.wait(wait_req)
rope_settling_time = action.get('settling_time', 1.0)
rospy.sleep(rope_settling_time)
def reset_rope(self, action_params: Dict):
reset = SetRopeStateRequest()
# TODO: rename this to rope endpoints reset positions or something
reset.left_gripper.x = numpify(
action_params['left_gripper_reset_position'][0])
reset.left_gripper.y = numpify(
action_params['left_gripper_reset_position'][1])
reset.left_gripper.z = numpify(
action_params['left_gripper_reset_position'][2])
reset.right_gripper.x = numpify(
action_params['right_gripper_reset_position'][0])
reset.right_gripper.y = numpify(
action_params['right_gripper_reset_position'][1])
reset.right_gripper.z = numpify(
action_params['right_gripper_reset_position'][2])
self.set_rope_state_srv(reset)
def sample_action(self,
action_rng: np.random.RandomState,
environment: Dict,
state: Dict,
action_params: Dict,
validate: bool,
stateless: Optional[bool] = False):
self.viz_action_sample_bbox(
self.left_gripper_bbox_pub,
self.get_action_sample_extent(action_params, 'left'))
self.viz_action_sample_bbox(
self.right_gripper_bbox_pub,
self.get_action_sample_extent(action_params, 'right'))
action = None
for _ in range(self.max_action_attempts):
# move in the same direction as the previous action with some probability
repeat_probability = action_params[
'repeat_delta_gripper_motion_probability']
if not stateless and self.last_action is not None and action_rng.uniform(
0, 1) < repeat_probability:
left_gripper_delta_position = self.last_action[
'left_gripper_delta_position']
right_gripper_delta_position = self.last_action[
'right_gripper_delta_position']
else:
# Sample a new random action
left_gripper_delta_position = self.sample_delta_position(
action_params, action_rng)
right_gripper_delta_position = self.sample_delta_position(
action_params, action_rng)
# Apply delta and check for out of bounds
left_gripper_position = state[
'left_gripper'] + left_gripper_delta_position
right_gripper_position = state[
'right_gripper'] + right_gripper_delta_position
action = {
'left_gripper_position': left_gripper_position,
'right_gripper_position': right_gripper_position,
'left_gripper_delta_position': left_gripper_delta_position,
'right_gripper_delta_position': right_gripper_delta_position,
}
if not validate or self.is_action_valid(action, action_params):
self.last_action = action
return action
rospy.logwarn(
"Could not find a valid action, executing an invalid one")
return action
def is_action_valid(self, action: Dict, action_params: Dict):
out_of_bounds = FloatingRopeScenario.grippers_out_of_bounds(
action['left_gripper_position'], action['right_gripper_position'],
action_params)
max_gripper_d = default_if_none(
action_params['max_distance_between_grippers'], 1000)
gripper_d = np.linalg.norm(action['left_gripper_position'] -
action['right_gripper_position'])
too_far = gripper_d > max_gripper_d
return not out_of_bounds and not too_far
def get_action_sample_extent(self, action_params: Dict, prefix: str):
k = prefix + 'gripper_action_sample_extent'
if k in action_params:
gripper_extent = np.array(action_params[k]).reshape([3, 2])
else:
gripper_extent = np.array(action_params['extent']).reshape([3, 2])
return gripper_extent
def viz_action_sample_bbox(self, gripper_bbox_pub: rospy.Publisher,
gripper_extent):
gripper_bbox_msg = extent_array_to_bbox(gripper_extent)
gripper_bbox_msg.header.frame_id = 'world'
gripper_bbox_pub.publish(gripper_bbox_msg)
def sample_delta_position(self, action_params, action_rng):
pitch = action_rng.uniform(-np.pi, np.pi)
yaw = action_rng.uniform(-np.pi, np.pi)
displacement = action_rng.uniform(
0, action_params['max_distance_gripper_can_move'])
rotation_matrix = transformations.euler_matrix(0, pitch, yaw)
gripper_delta_position_homo = rotation_matrix @ np.array(
[1, 0, 0, 1]) * displacement
gripper_delta_position = gripper_delta_position_homo[:3]
return gripper_delta_position
@staticmethod
def grippers_out_of_bounds(left_gripper, right_gripper,
action_params: Dict):
left_gripper_extent = action_params[
'left_gripper_action_sample_extent']
right_gripper_extent = action_params[
'right_gripper_action_sample_extent']
return FloatingRopeScenario.is_out_of_bounds(left_gripper, left_gripper_extent) \
or FloatingRopeScenario.is_out_of_bounds(right_gripper, right_gripper_extent)
@staticmethod
def is_out_of_bounds(p, extent):
x, y, z = p
x_min, x_max, y_min, y_max, z_min, z_max = extent
return x < x_min or x > x_max \
or y < y_min or y > y_max \
or z < z_min or z > z_max
@staticmethod
def interpolate(start_state, end_state, step_size=0.05):
left_gripper_start = np.array(start_state['left_gripper'])
left_gripper_end = np.array(end_state['left_gripper'])
right_gripper_start = np.array(start_state['right_gripper'])
right_gripper_end = np.array(end_state['right_gripper'])
left_gripper_delta = left_gripper_end - left_gripper_start
right_gripper_delta = right_gripper_end - right_gripper_start
left_gripper_steps = np.round(
np.linalg.norm(left_gripper_delta) / step_size).astype(np.int64)
right_gripper_steps = np.round(
np.linalg.norm(right_gripper_delta) / step_size).astype(np.int64)
steps = max(left_gripper_steps, right_gripper_steps)
interpolated_actions = []
for t in np.linspace(step_size, 1, steps):
left_gripper_i = left_gripper_start + left_gripper_delta * t
right_gripper_i = right_gripper_start + right_gripper_delta * t
action = {
'left_gripper_position': left_gripper_i,
'right_gripper_position': right_gripper_i,
}
interpolated_actions.append(action)
return interpolated_actions
@staticmethod
def robot_name():
return "rope_3d"
def randomize_environment(self, env_rng, params: Dict):
pass
@staticmethod
def put_state_robot_frame(state: Dict):
rope = state[rope_key_name]
rope_points_shape = rope.shape[:-1].as_list() + [-1, 3]
rope_points = tf.reshape(rope, rope_points_shape)
# This assumes robot is at 0 0 0
robot_position = tf.constant([[0, 0, 0]], tf.float32)
left_gripper_robot = state['left_gripper']
right_gripper_robot = state['right_gripper']
rope_points_robot = rope_points - tf.expand_dims(robot_position,
axis=-2)
rope_robot = tf.reshape(rope_points_robot, rope.shape)
return {
'left_gripper': left_gripper_robot,
'right_gripper': right_gripper_robot,
rope_key_name: rope_robot,
}
@staticmethod
def put_state_local_frame(state: Dict):
rope = state[rope_key_name]
rope_points_shape = rope.shape[:-1].as_list() + [-1, 3]
rope_points = tf.reshape(rope, rope_points_shape)
center = tf.reduce_mean(rope_points, axis=-2)
left_gripper_local = state['left_gripper'] - center
right_gripper_local = state['right_gripper'] - center
rope_points_local = rope_points - tf.expand_dims(center, axis=-2)
rope_local = tf.reshape(rope_points_local, rope.shape)
return {
'left_gripper': left_gripper_local,
'right_gripper': right_gripper_local,
rope_key_name: rope_local,
}
@staticmethod
def local_environment_center_differentiable(state):
rope_vector = state[rope_key_name]
rope_points = tf.reshape(rope_vector, [rope_vector.shape[0], -1, 3])
center = tf.reduce_mean(rope_points, axis=1)
return center
@staticmethod
def apply_local_action_at_state(state, local_action):
return {
'left_gripper_position':
state['left_gripper'] + local_action['left_gripper_delta'],
'right_gripper_position':
state['right_gripper'] + local_action['right_gripper_delta']
}
@staticmethod
def add_noise(action: Dict, noise_rng: np.random.RandomState):
left_gripper_noise = noise_rng.normal(scale=0.01, size=[3])
right_gripper_noise = noise_rng.normal(scale=0.01, size=[3])
return {
'left_gripper_position':
action['left_gripper_position'] + left_gripper_noise,
'right_gripper_position':
action['right_gripper_position'] + right_gripper_noise
}
@staticmethod
def integrate_dynamics(s_t: Dict, delta_s_t: Dict):
return {k: s_t[k] + delta_s_t[k] for k in s_t.keys()}
@staticmethod
def put_action_local_frame(state: Dict, action: Dict):
target_left_gripper_position = action['left_gripper_position']
target_right_gripper_position = action['right_gripper_position']
n_action = target_left_gripper_position.shape[1]
current_left_gripper_point = state['left_gripper']
current_right_gripper_point = state['right_gripper']
left_gripper_delta = target_left_gripper_position - current_left_gripper_point[:, :
n_action]
right_gripper_delta = target_right_gripper_position - current_right_gripper_point[:, :
n_action]
return {
'left_gripper_delta': left_gripper_delta,
'right_gripper_delta': right_gripper_delta,
}
@staticmethod
def state_to_gripper_position(state: Dict):
gripper_position1 = np.reshape(state['left_gripper'], [3])
gripper_position2 = np.reshape(state['right_gripper'], [3])
return gripper_position1, gripper_position2
def get_cdcpd_state(self):
cdcpd_msg: PointCloud2 = self.cdcpd_listener.get()
# transform into robot-frame
transform = self.tf.get_transform_msg("robot_root",
"kinect2_rgb_optical_frame")
cdcpd_points_robot_frame = tf2_sensor_msgs.do_transform_cloud(
cdcpd_msg, transform)
cdcpd_points_array = ros_numpy.numpify(cdcpd_points_robot_frame)
x = cdcpd_points_array['x']
y = cdcpd_points_array['y']
z = cdcpd_points_array['z']
points = np.stack([x, y, z], axis=-1)
cdcpd_vector = points.flatten()
return cdcpd_vector
def get_rope_state(self):
rope_res = self.get_rope_srv(GetRopeStateRequest())
rope_state_vector = []
for p in rope_res.positions:
rope_state_vector.append(p.x)
rope_state_vector.append(p.y)
rope_state_vector.append(p.z)
rope_velocity_vector = []
for v in rope_res.velocities:
rope_velocity_vector.append(v.x)
rope_velocity_vector.append(v.y)
rope_velocity_vector.append(v.z)
return rope_state_vector
def get_rope_point_positions(self):
# NOTE: consider getting rid of this message type/service just use rope state [0] and rope state [-1]
# although that looses semantic meaning and means hard-coding indices a lot...
left_res: GetPosition3DResponse = self.pos3d.get(
scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'left_gripper'))
left_rope_point_position = ros_numpy.numpify(left_res.pos)
right_res: GetPosition3DResponse = self.pos3d.get(
scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'right_gripper'))
right_rope_point_position = ros_numpy.numpify(right_res.pos)
return left_rope_point_position, right_rope_point_position
def get_state(self):
color_depth_cropped = self.get_rgbd()
rope_state_vector = self.get_rope_state()
cdcpd_vector = self.get_cdcpd_state()
left_rope_point_position, right_rope_point_position = self.get_rope_point_positions(
)
return {
'left_gripper': left_rope_point_position,
'right_gripper': right_rope_point_position,
'gt_rope': np.array(rope_state_vector, np.float32),
rope_key_name: np.array(cdcpd_vector, np.float32),
'rgbd': color_depth_cropped,
}
def get_rgbd(self):
color_msg: Image = self.color_image_listener.get()
depth_msg = self.depth_image_listener.get()
depth = np.expand_dims(ros_numpy.numpify(depth_msg), axis=-1)
bgr = ros_numpy.numpify(color_msg)
rgb = np.flip(bgr, axis=2)
# NaN Depths means out of range, so clip to the max range
depth = np.clip(np.nan_to_num(depth, nan=KINECT_MAX_DEPTH), 0,
KINECT_MAX_DEPTH)
rgbd = np.concatenate([rgb, depth], axis=2)
box = tf.convert_to_tensor([
self.crop_region['min_y'] / rgb.shape[0],
self.crop_region['min_x'] / rgb.shape[1],
self.crop_region['max_y'] / rgb.shape[0],
self.crop_region['max_x'] / rgb.shape[1]
],
dtype=tf.float32)
# this operates on a batch
rgbd_cropped = tf.image.crop_and_resize(
image=tf.expand_dims(rgbd, axis=0),
boxes=tf.expand_dims(box, axis=0),
box_indices=[0],
crop_size=[self.IMAGE_H, self.IMAGE_W])
rgbd_cropped = remove_batch(rgbd_cropped)
def _debug_show_image(_rgb_depth_cropped):
import matplotlib.pyplot as plt
plt.imshow(tf.cast(_rgb_depth_cropped[:, :, :3], tf.int32))
plt.show()
# BEGIN DEBUG
# _debug_show_image(rgbd_cropped)
# END DEBUG
return rgbd_cropped.numpy()
def observations_description(self) -> Dict:
return {
'left_gripper': 3,
'right_gripper': 3,
'rgbd': [self.IMAGE_H, self.IMAGE_W, 4],
}
@staticmethod
def states_description() -> Dict:
return {}
@staticmethod
def observation_features_description() -> Dict:
return {
rope_key_name: FloatingRopeScenario.n_links * 3,
'cdcpd': FloatingRopeScenario.n_links * 3,
}
@staticmethod
def actions_description() -> Dict:
# should match the keys of the dict return from action_to_dataset_action
return {
'left_gripper_position': 3,
'right_gripper_position': 3,
}
@staticmethod
def state_to_points_for_cc(state: Dict):
return state[rope_key_name].reshape(-1, 3)
def __repr__(self):
return "DualFloatingGripperRope"
def simple_name(self):
return "dual_floating"
@staticmethod
def sample_gripper_goal(environment: Dict, rng: np.random.RandomState,
planner_params: Dict):
env_inflated = inflate_tf_3d(
env=environment['env'],
radius_m=planner_params['goal_params']['threshold'],
res=environment['res'])
goal_extent = planner_params['goal_params']['extent']
while True:
extent = np.array(goal_extent).reshape(3, 2)
left_gripper = rng.uniform(extent[:, 0], extent[:, 1])
right_gripper = rng.uniform(extent[:, 0], extent[:, 1])
goal = {
'left_gripper': left_gripper,
'right_gripper': right_gripper,
}
row1, col1, channel1 = grid_utils.point_to_idx_3d_in_env(
left_gripper[0], left_gripper[1], left_gripper[2], environment)
row2, col2, channel2 = grid_utils.point_to_idx_3d_in_env(
right_gripper[0], right_gripper[1], right_gripper[2],
environment)
collision1 = env_inflated[row1, col1, channel1] > 0.5
collision2 = env_inflated[row2, col2, channel2] > 0.5
if not collision1 and not collision2:
return goal
def sample_goal(self, environment: Dict, rng: np.random.RandomState,
planner_params: Dict):
goal_type = planner_params['goal_params']['goal_type']
if goal_type == 'midpoint':
return self.sample_midpoint_goal(environment, rng, planner_params)
else:
raise NotImplementedError(
planner_params['goal_params']['goal_type'])
@staticmethod
def distance_to_gripper_goal(state: Dict, goal: Dict):
left_gripper = state['left_gripper']
right_gripper = state['right_gripper']
distance1 = np.linalg.norm(goal['left_gripper'] - left_gripper)
distance2 = np.linalg.norm(goal['right_gripper'] - right_gripper)
return max(distance1, distance2)
def sample_midpoint_goal(self, environment: Dict,
rng: np.random.RandomState, planner_params: Dict):
goal_extent = planner_params['goal_params']['extent']
if environment == {}:
rospy.loginfo("Assuming no obstacles in the environment")
extent = np.array(goal_extent).reshape(3, 2)
p = rng.uniform(extent[:, 0], extent[:, 1])
goal = {'midpoint': p}
return goal
env_inflated = inflate_tf_3d(
env=environment['env'],
radius_m=planner_params['goal_params']['threshold'],
res=environment['res'])
# DEBUG visualize the inflated env
# from copy import deepcopy
# environment_ = deepcopy(environment)
# environment_['env'] = env_inflated
# self.plot_environment_rviz(environment_)
# END DEBUG
while True:
extent = np.array(goal_extent).reshape(3, 2)
p = rng.uniform(extent[:, 0], extent[:, 1])
goal = {'midpoint': p}
row, col, channel = grid_utils.point_to_idx_3d_in_env(
p[0], p[1], p[2], environment)
collision = env_inflated[row, col, channel] > 0.5
if not collision:
return goal
@staticmethod
def distance_grippers_and_any_point_goal(state: Dict, goal: Dict):
rope_points = np.reshape(state[rope_key_name], [-1, 3])
# well ok not _any_ node, but ones near the middle
n_from_ends = 5
distances = np.linalg.norm(np.expand_dims(goal['point'], axis=0) -
rope_points,
axis=1)[n_from_ends:-n_from_ends]
rope_distance = np.min(distances)
left_gripper = state['left_gripper']
right_gripper = state['right_gripper']
distance1 = np.linalg.norm(goal['left_gripper'] - left_gripper)
distance2 = np.linalg.norm(goal['right_gripper'] - right_gripper)
return max(max(distance1, distance2), rope_distance)
@staticmethod
def distance_to_any_point_goal(state: Dict, goal: Dict):
rope_points = np.reshape(state[rope_key_name], [-1, 3])
# well ok not _any_ node, but ones near the middle
n_from_ends = 7
distances = np.linalg.norm(np.expand_dims(goal['point'], axis=0) -
rope_points,
axis=1)[n_from_ends:-n_from_ends]
min_distance = np.min(distances)
return min_distance
@staticmethod
def distance_to_midpoint_goal(state: Dict, goal: Dict):
rope_points = np.reshape(state[rope_key_name], [-1, 3])
rope_midpoint = rope_points[int(FloatingRopeScenario.n_links / 2)]
distance = np.linalg.norm(goal['midpoint'] - rope_midpoint)
return distance
def classifier_distance(self, s1: Dict, s2: Dict):
model_error = np.linalg.norm(s1[rope_key_name] - s2[rope_key_name],
axis=-1)
# labeling_states = s1['rope']
# labeling_predicted_states = s2['rope']
# points_shape = labeling_states.shape.as_list()[:2] + [-1, 3]
# labeling_points = tf.reshape(labeling_states, points_shape)
# labeling_predicted_points = tf.reshape(labeling_predicted_states, points_shape)
# model_error = tf.reduce_mean(tf.linalg.norm(labeling_points - labeling_predicted_points, axis=-1), axis=-1)
return model_error
def compute_label(self, actual: Dict, predicted: Dict,
labeling_params: Dict):
# NOTE: this should be using the same distance metric as the planning, which should also be the same as the labeling
# done when making the classifier dataset
actual_rope = np.array(actual["rope"])
predicted_rope = np.array(predicted["rope"])
model_error = np.linalg.norm(actual_rope - predicted_rope)
threshold = labeling_params['threshold']
is_close = model_error < threshold
return is_close
def distance_to_goal(self, state, goal):
if 'type' not in goal or goal['type'] == 'midpoint':
return self.distance_to_midpoint_goal(state, goal)
elif goal['type'] == 'any_point':
return self.distance_to_any_point_goal(state, goal)
elif goal['type'] == 'grippers':
return self.distance_to_gripper_goal(state, goal)
elif goal['type'] == 'grippers_and_point':
return self.distance_grippers_and_any_point_goal(state, goal)
else:
raise NotImplementedError()
def plot_goal_rviz(self,
goal: Dict,
goal_threshold: float,
actually_at_goal: Optional[bool] = None):
if actually_at_goal:
r = 0.4
g = 0.8
b = 0.4
a = 0.6
else:
r = 0.5
g = 0.3
b = 0.8
a = 0.6
goal_marker_msg = MarkerArray()
if 'midpoint' in goal:
midpoint_marker = Marker()
midpoint_marker.scale.x = goal_threshold * 2
midpoint_marker.scale.y = goal_threshold * 2
midpoint_marker.scale.z = goal_threshold * 2
midpoint_marker.action = Marker.ADD
midpoint_marker.type = Marker.SPHERE
midpoint_marker.header.frame_id = "world"
midpoint_marker.header.stamp = rospy.Time.now()
midpoint_marker.ns = 'goal'
midpoint_marker.id = 0
midpoint_marker.color.r = r
midpoint_marker.color.g = g
midpoint_marker.color.b = b
midpoint_marker.color.a = a
midpoint_marker.pose.position.x = goal['midpoint'][0]
midpoint_marker.pose.position.y = goal['midpoint'][1]
midpoint_marker.pose.position.z = goal['midpoint'][2]
midpoint_marker.pose.orientation.w = 1
goal_marker_msg.markers.append(midpoint_marker)
if 'point' in goal:
point_marker = Marker()
point_marker.scale.x = goal_threshold * 2
point_marker.scale.y = goal_threshold * 2
point_marker.scale.z = goal_threshold * 2
point_marker.action = Marker.ADD
point_marker.type = Marker.SPHERE
point_marker.header.frame_id = "world"
point_marker.header.stamp = rospy.Time.now()
point_marker.ns = 'goal'
point_marker.id = 0
point_marker.color.r = r
point_marker.color.g = g
point_marker.color.b = b
point_marker.color.a = a
point_marker.pose.position.x = goal['point'][0]
point_marker.pose.position.y = goal['point'][1]
point_marker.pose.position.z = goal['point'][2]
point_marker.pose.orientation.w = 1
goal_marker_msg.markers.append(point_marker)
if 'left_gripper' in goal:
left_gripper_marker = Marker()
left_gripper_marker.scale.x = goal_threshold * 2
left_gripper_marker.scale.y = goal_threshold * 2
left_gripper_marker.scale.z = goal_threshold * 2
left_gripper_marker.action = Marker.ADD
left_gripper_marker.type = Marker.SPHERE
left_gripper_marker.header.frame_id = "world"
left_gripper_marker.header.stamp = rospy.Time.now()
left_gripper_marker.ns = 'goal'
left_gripper_marker.id = 1
left_gripper_marker.color.r = r
left_gripper_marker.color.g = g
left_gripper_marker.color.b = b
left_gripper_marker.color.a = a
left_gripper_marker.pose.position.x = goal['left_gripper'][0]
left_gripper_marker.pose.position.y = goal['left_gripper'][1]
left_gripper_marker.pose.position.z = goal['left_gripper'][2]
left_gripper_marker.pose.orientation.w = 1
goal_marker_msg.markers.append(left_gripper_marker)
if 'right_gripper' in goal:
right_gripper_marker = Marker()
right_gripper_marker.scale.x = goal_threshold * 2
right_gripper_marker.scale.y = goal_threshold * 2
right_gripper_marker.scale.z = goal_threshold * 2
right_gripper_marker.action = Marker.ADD
right_gripper_marker.type = Marker.SPHERE
right_gripper_marker.header.frame_id = "world"
right_gripper_marker.header.stamp = rospy.Time.now()
right_gripper_marker.ns = 'goal'
right_gripper_marker.id = 2
right_gripper_marker.color.r = r
right_gripper_marker.color.g = g
right_gripper_marker.color.b = b
right_gripper_marker.color.a = a
right_gripper_marker.pose.position.x = goal['right_gripper'][0]
right_gripper_marker.pose.position.y = goal['right_gripper'][1]
right_gripper_marker.pose.position.z = goal['right_gripper'][2]
right_gripper_marker.pose.orientation.w = 1
goal_marker_msg.markers.append(right_gripper_marker)
self.state_viz_pub.publish(goal_marker_msg)
def plot_goal_boxes(self,
goal: Dict,
goal_threshold: float,
actually_at_goal: Optional[bool] = None):
if actually_at_goal:
r = 0.4
g = 0.8
b = 0.4
a = 0.6
else:
r = 0.5
g = 0.3
b = 0.8
a = 0.6
goal_marker_msg = MarkerArray()
if 'point_box' in goal:
point_marker = make_box_marker_from_extents(goal['point_box'])
point_marker.header.frame_id = "world"
point_marker.header.stamp = rospy.Time.now()
point_marker.ns = 'goal'
point_marker.id = 0
point_marker.color.r = r
point_marker.color.g = g
point_marker.color.b = b
point_marker.color.a = a
goal_marker_msg.markers.append(point_marker)
if 'left_gripper_box' in goal:
left_gripper_marker = make_box_marker_from_extents(
goal['left_gripper_box'])
left_gripper_marker.header.frame_id = "world"
left_gripper_marker.header.stamp = rospy.Time.now()
left_gripper_marker.ns = 'goal'
left_gripper_marker.id = 1
left_gripper_marker.color.r = r
left_gripper_marker.color.g = g
left_gripper_marker.color.b = b
left_gripper_marker.color.a = a
goal_marker_msg.markers.append(left_gripper_marker)
if 'right_gripper_box' in goal:
right_gripper_marker = make_box_marker_from_extents(
goal['right_gripper_box'])
right_gripper_marker.header.frame_id = "world"
right_gripper_marker.header.stamp = rospy.Time.now()
right_gripper_marker.ns = 'goal'
right_gripper_marker.id = 2
right_gripper_marker.color.r = r
right_gripper_marker.color.g = g
right_gripper_marker.color.b = b
right_gripper_marker.color.a = a
goal_marker_msg.markers.append(right_gripper_marker)
self.state_viz_pub.publish(goal_marker_msg)
@staticmethod
def dynamics_loss_function(dataset_element, predictions):
return dynamics_loss_function(dataset_element, predictions)
@staticmethod
def dynamics_metrics_function(dataset_element, predictions):
return dynamics_points_metrics_function(dataset_element, predictions)
def plot_tree_action(self, state: Dict, action: Dict, **kwargs):
r = kwargs.pop("r", 0.2)
g = kwargs.pop("g", 0.2)
b = kwargs.pop("b", 0.8)
a = kwargs.pop("a", 1.0)
ig = marker_index_generator(self.tree_action_idx)
idx1 = next(ig)
idx2 = next(ig)
self.plot_action_rviz(state,
action,
label='tree',
color=[r, g, b, a],
idx1=idx1,
idx2=idx2,
**kwargs)
self.tree_action_idx += 1
def plot_state_rviz(self, state: Dict, label: str, **kwargs):
r, g, b, a = colors.to_rgba(kwargs.get("color", "r"))
idx = kwargs.get("idx", 0)
msg = MarkerArray()
ig = marker_index_generator(idx)
if 'gt_rope' in state:
rope_points = np.reshape(state['gt_rope'], [-1, 3])
markers = make_rope_marker(rope_points, 'world',
label + "_gt_rope", next(ig), r, g, b,
a)
msg.markers.extend(markers)
if 'rope' in state:
rope_points = np.reshape(state['rope'], [-1, 3])
markers = make_rope_marker(rope_points, 'world', label + "_rope",
next(ig), r, g, b, a)
msg.markers.extend(markers)
if add_predicted('rope') in state:
rope_points = np.reshape(state[add_predicted('rope')], [-1, 3])
markers = make_rope_marker(rope_points, 'world',
label + "_pred_rope", next(ig), r, g, b,
a, Marker.CUBE_LIST)
msg.markers.extend(markers)
if 'left_gripper' in state:
r = 0.2
g = 0.2
b = 0.8
left_gripper_sphere = make_gripper_marker(state['left_gripper'],
next(ig), r, g, b, a,
label + '_l',
Marker.SPHERE)
msg.markers.append(left_gripper_sphere)
if 'right_gripper' in state:
r = 0.8
g = 0.8
b = 0.2
right_gripper_sphere = make_gripper_marker(state['right_gripper'],
next(ig), r, g, b, a,
label + "_r",
Marker.SPHERE)
msg.markers.append(right_gripper_sphere)
if add_predicted('left_gripper') in state:
r = 0.2
g = 0.2
b = 0.8
lgpp = state[add_predicted('left_gripper')]
left_gripper_sphere = make_gripper_marker(lgpp, next(ig), r, g, b,
a, label + "_lp",
Marker.CUBE)
msg.markers.append(left_gripper_sphere)
if add_predicted('right_gripper') in state:
r = 0.8
g = 0.8
b = 0.2
rgpp = state[add_predicted('right_gripper')]
right_gripper_sphere = make_gripper_marker(rgpp, next(ig), r, g, b,
a, label + "_rp",
Marker.CUBE)
msg.markers.append(right_gripper_sphere)
s = kwargs.get("scale", 1.0)
msg = scale_marker_array(msg, s)
self.state_viz_pub.publish(msg)
if in_maybe_predicted('rgbd', state):
publish_color_image(self.state_color_viz_pub,
state['rgbd'][:, :, :3])
publish_depth_image(self.state_depth_viz_pub, state['rgbd'][:, :,
3])
if add_predicted('stdev') in state:
stdev_t = state[add_predicted('stdev')][0]
self.plot_stdev(stdev_t)
if 'error' in state:
error_msg = Float32()
error_t = state['error']
error_msg.data = error_t
self.error_pub.publish(error_msg)
def plot_action_rviz(self,
state: Dict,
action: Dict,
label: str = 'action',
**kwargs):
state_action = {}
state_action.update(state)
state_action.update(action)
self.plot_action_rviz_internal(state_action, label=label, **kwargs)
def plot_action_rviz_internal(self, data: Dict, label: str, **kwargs):
r, g, b, a = colors.to_rgba(kwargs.get("color", "b"))
s1 = np.reshape(get_maybe_predicted(data, 'left_gripper'), [3])
s2 = np.reshape(get_maybe_predicted(data, 'right_gripper'), [3])
a1 = np.reshape(get_maybe_predicted(data, 'left_gripper_position'),
[3])
a2 = np.reshape(get_maybe_predicted(data, 'right_gripper_position'),
[3])
idx = kwargs.pop("idx", None)
ig = marker_index_generator(idx)
if idx is not None:
idx1 = next(ig)
idx2 = next(ig)
else:
idx1 = kwargs.pop("idx1", 0)
idx2 = kwargs.pop("idx2", 1)
msg = MarkerArray()
msg.markers.append(
rviz_arrow(s1, a1, r, g, b, a, idx=idx1, label=label, **kwargs))
msg.markers.append(
rviz_arrow(s2, a2, r, g, b, a, idx=idx2, label=label, **kwargs))
self.action_viz_pub.publish(msg)
def register_fake_grasping(self):
register_left_req = RegisterPosition3DControllerRequest()
register_left_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"left_gripper")
register_left_req.controller_type = "kinematic"
self.pos3d.register(register_left_req)
register_right_req = RegisterPosition3DControllerRequest()
register_right_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"right_gripper")
register_right_req.controller_type = "kinematic"
self.pos3d.register(register_right_req)
def make_rope_endpoints_follow_gripper(self):
left_follow_req = Position3DFollowRequest()
left_follow_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"left_gripper")
left_follow_req.frame_id = "left_tool"
self.pos3d.follow(left_follow_req)
right_follow_req = Position3DFollowRequest()
right_follow_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"right_gripper")
right_follow_req.frame_id = "right_tool"
self.pos3d.follow(right_follow_req)
def load_rosparam(self,
context,
ros_config,
cmd,
param,
file_,
text,
verbose=True):
"""
Load rosparam setting
@param context: Loader context
@type context: L{LoaderContext}
@param ros_config: launch configuration
@type ros_config: L{ROSLaunchConfig}
@param cmd: 'load', 'dump', or 'delete'
@type cmd: str
@param file_: filename for rosparam to use or None
@type file_: str
@param text: text for rosparam to load. Ignored if file_ is set.
@type text: str
@raise ValueError: if parameters cannot be processed into valid rosparam setting
"""
if not cmd in ('load', 'dump', 'delete'):
raise ValueError("command must be 'load', 'dump', or 'delete'")
if file_ is not None:
if cmd == 'load' and not os.path.isfile(file_):
raise ValueError("file does not exist [%s]" % file_)
if cmd == 'delete':
raise ValueError(
"'file' attribute is invalid with 'delete' command.")
full_param = ns_join(context.ns, param) if param else context.ns
if cmd == 'dump':
ros_config.add_executable(
RosbinExecutable('rosparam', (cmd, file_, full_param),
PHASE_SETUP))
elif cmd == 'delete':
ros_config.add_executable(
RosbinExecutable('rosparam', (cmd, full_param), PHASE_SETUP))
elif cmd == 'load':
# load YAML text
if file_:
with open(file_, 'r') as f:
text = f.read()
# parse YAML text
# - lazy import
global yaml
if yaml is None:
import yaml
# - lazy import: we have to import rosparam in oder to to configure the YAML constructors
global rosparam
if rosparam is None:
import rosparam
try:
data = yaml.load(text)
# #3162: if there is no YAML, load() will return an
# empty string. We want an empty dictionary instead
# for our representation of empty.
if data is None:
data = {}
except yaml.MarkedYAMLError as e:
if not file_:
raise ValueError(
"Error within YAML block:\n\t%s\n\nYAML is:\n%s" %
(str(e), text))
else:
raise ValueError("file %s contains invalid YAML:\n%s" %
(file_, str(e)))
except Exception as e:
if not file_:
raise ValueError("invalid YAML: %s\n\nYAML is:\n%s" %
(str(e), text))
else:
raise ValueError("file %s contains invalid YAML:\n%s" %
(file_, str(e)))
# 'param' attribute is required for non-dictionary types
if not param and type(data) != dict:
raise ValueError(
"'param' attribute must be set for non-dictionary values")
self.add_param(ros_config, full_param, data, verbose=verbose)
else:
raise ValueError("unknown command %s" % cmd)
def test_nsjoin():
from rosgraph.names import ns_join
# private and global names cannot be joined
assert '~name' == ns_join('/foo', '~name')
assert '/name' == ns_join('/foo', '/name')
assert '~name' == ns_join('~', '~name')
assert '/name' == ns_join('/', '/name')
# ns can be '~' or '/'
assert '~name' == ns_join('~', 'name')
assert '/name' == ns_join('/', 'name')
assert '/ns/name' == ns_join('/ns', 'name')
assert '/ns/name' == ns_join('/ns/', 'name')
assert '/ns/ns2/name' == ns_join('/ns', 'ns2/name')
assert '/ns/ns2/name' == ns_join('/ns/', 'ns2/name')
# allow ns to be empty
assert 'name' == ns_join('', 'name')
