def main(self, *, args): # noqa: D102
with NodeStrategy(args) as node:
node_names = get_node_names(
node=node, include_hidden_nodes=args.include_hidden_nodes)
if args.node_name not in node_names:
return 'Node not found'
with DirectNode(args) as node:
parameter = Parameter()
Parameter.name = args.name
parameter.value = get_parameter_value(string_value=args.value)
response = call_set_parameters(node=node,
node_name=args.node_name,
parameters=[parameter])
# output response
assert len(response.results) == 1
result = response.results[0]
if result.successful:
msg = 'Set parameter successful'
if result.reason:
msg += ': ' + result.reason
print(msg)
else:
msg = 'Setting parameter failed'
if result.reason:
msg += ': ' + result.reason
print(msg, file=sys.stderr)
def main(self, *, args): # noqa: D102
with NodeStrategy(args) as node:
node_names = get_node_names(
node=node, include_hidden_nodes=args.include_hidden_nodes)
node_name = get_absolute_node_name(args.node_name)
if node_name not in [n.full_name for n in node_names]:
return 'Node not found'
with DirectNode(args) as node:
parameter = Parameter()
Parameter.name = args.name
value = ParameterValue()
value.type = ParameterType.PARAMETER_NOT_SET
parameter.value = value
response = call_set_parameters(node=node,
node_name=args.node_name,
parameters=[parameter])
# output response
assert len(response.results) == 1
result = response.results[0]
if result.successful:
msg = 'Deleted parameter successfully'
if result.reason:
msg += ': ' + result.reason
print(msg)
else:
msg = 'Deleting parameter failed'
if result.reason:
msg += ': ' + result.reason
print(msg, file=sys.stderr)
def execution(self):
# Set calibration state to success
req = SetParameters.Request()
param = Parameter()
param.name = "calibration_state"
param.value.type = ParameterType.PARAMETER_INTEGER
param.value.integer_value = 4
req.parameters.append(param)
future = self.client_param.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def send_request(self, option, value):
self.req_ = SetParameters.Request()
# Create one parameter:
param = Parameter()
if (option == 1):
param.value.type = ParameterType.PARAMETER_STRING
param.value.string_value = value
else:
param.value.type = ParameterType.PARAMETER_BOOL
v = value == "True"
param.value.bool_value = v
if (option == 1):
param.name = "model_path"
elif (option == 2):
param.name = "display_masks"
elif (option == 3):
param.name = "display_bboxes"
elif (option == 4):
param.name = "display_text"
elif (option == 5):
param.name = "display_scores"
elif (option == 6):
param.name = "display_fps"
print(param.name)
# Append to de list:
self.req_.parameters.append(param)
self.future = self.client_srv_.call_async(self.req_)
def parse_parameter_dict(*, namespace, parameter_dict):
parameters = []
for param_name, param_value in parameter_dict.items():
full_param_name = namespace + param_name
# Unroll nested parameters
if type(param_value) == dict:
parameters += parse_parameter_dict(
namespace=full_param_name + PARAMETER_SEPARATOR_STRING,
parameter_dict=param_value)
else:
parameter = Parameter()
parameter.name = full_param_name
parameter.value = get_parameter_value(string_value=str(param_value))
parameters.append(parameter)
return parameters
def __init__(self, robot_name, wandb=False, config_name="optimization"):
super().__init__(robot_name, wandb=wandb)
self.current_speed = None
self.last_time = 0
self.number_of_iterations = 100
self.time_limit = 10
# needs to be specified by subclasses
self.directions = None
self.reset_height_offset = None
self.reset_rpy_offset = [0, 0, 0]
self.last_yaw = 0
self.summed_yaw = 0
# load moveit config values
moveit_parameters = load_moveit_parameter(self.robot_name)
# load walk params
self.walk_parameters = get_parameters_from_ros_yaml(
"walking", f"{get_package_share_directory('bitbots_quintic_walk')}"
f"/config/{config_name}.yaml",
use_wildcard=True)
# activate IK reset only for wolfgang
self.walk_parameters.append(
Parameter(name="node.ik_reset",
value=ParameterValue(
bool_value=self.robot_name == "wolfgang")))
# create walk as python class to call it later
self.walk = PyWalk(self.namespace,
self.walk_parameters + moveit_parameters)
def send_request_to_change_parameters(
self, # Arguments are alphabetically sorted
expect,
frame_rate: float,
output_directory: Path,
real_time_factor: float,
scenario: Path,
):
"""Send request to change scenario interpreter's parameters."""
request = rcl_interfaces.srv.SetParameters.Request()
request.parameters = [
Parameter(
name="intended_result",
value=ParameterValue(type=ParameterType.PARAMETER_STRING,
string_value=str(expect.name)),
),
Parameter(
name="osc_path",
value=ParameterValue(type=ParameterType.PARAMETER_STRING,
string_value=str(scenario)),
),
Parameter(
name="output_directory",
value=ParameterValue(
type=ParameterType.PARAMETER_STRING,
string_value=str(output_directory),
),
),
Parameter(
name="local_real_time_factor",
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE,
double_value=real_time_factor),
),
Parameter(
name="local_frame_rate",
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE,
double_value=frame_rate),
),
]
future = self.client_set_parameters.call_async(request)
rclpy.spin_until_future_complete(self,
future,
timeout_sec=self.timeout_sec)
return future
def _parse_parameter_dict(namespace, parameter_dict):
"""
Parses the parameters dictionary into a list of Parameters
@param namespace: Parameters namespace
@param parameter_dict: The ros__parameters key of the yaml
"""
parameters = []
for param_name, param_value in parameter_dict.items():
full_param_name = namespace + param_name
# Unroll nested parameters
if type(param_value) == dict:
parameters += _parse_parameter_dict(namespace=full_param_name +
PARAMETER_SEPARATOR_STRING,
parameter_dict=param_value)
else:
parameter = Parameter()
parameter.name = full_param_name
parameter.value = _get_parameter_value(
string_value=str(param_value))
parameters.append(parameter)
return parameters
def _set_param(node_name, name, value, parameter_type=None):
"""
Internal helper function for set_param.
Attempts to set the given parameter in the target node with the desired value,
deducing the parameter type if it's not specified.
parameter_type allows forcing a type for the given value; this is useful to delete parameters.
"""
parameter = Parameter()
parameter.name = name
if parameter_type is None:
parameter.value = get_parameter_value(string_value=value)
else:
parameter.value = ParameterValue()
parameter.value.type = parameter_type
if parameter_type != ParameterType.PARAMETER_NOT_SET:
setattr(parameter.value, _parameter_type_mapping[parameter_type])
try:
# call_get_parameters will fail if node does not exist.
call_set_parameters(node=_node,
node_name=node_name,
parameters=[parameter])
except:
pass
def load_moveit_parameter(robot_name):
moveit_parameters = get_parameters_from_plain_yaml(
f"{get_package_share_directory(f'{robot_name}_moveit_config')}/config/kinematics.yaml",
"robot_description_kinematics.")
robot_description = ParameterMsg()
robot_description.name = "robot_description"
robot_description.value = ParameterValueMsg(
string_value=read_urdf(robot_name),
type=ParameterTypeMsg.PARAMETER_STRING)
moveit_parameters.append(robot_description)
robot_description_semantic = ParameterMsg()
robot_description_semantic.name = "robot_description_semantic"
with open(
f"{get_package_share_directory(f'{robot_name}_moveit_config')}/config/{robot_name}.srdf",
"r") as file:
value = file.read()
robot_description_semantic.value = ParameterValueMsg(
string_value=value, type=ParameterTypeMsg.PARAMETER_STRING)
moveit_parameters.append(robot_description_semantic)
return moveit_parameters
def __init__(self):
super().__init__('experiments')
# Set up experiments
self._experiments = [
# Experiment(note='simple controller', mode=Control.AUV_SEQUENCE, count=1, base_params=[
# Parameter(name='auv_z_target',
# value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=-1.5)),
# Parameter(name='auv_controller',
# value=ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=0)),
# ], filter_params=[
# Parameter(name='four_dof',
# value=ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=True)),
# ]),
Experiment(note='ignore estimate controller',
mode=Control.AUV_SEQUENCE,
count=1,
base_params=[
Parameter(name='auv_z_target',
value=ParameterValue(
type=ParameterType.PARAMETER_DOUBLE,
double_value=-1.5)),
Parameter(name='auv_controller',
value=ParameterValue(
type=ParameterType.PARAMETER_INTEGER,
integer_value=1)),
],
filter_params=[
Parameter(name='four_dof',
value=ParameterValue(
type=ParameterType.PARAMETER_BOOL,
bool_value=True)),
]),
# Experiment(note='six DoF filter', mode=Control.AUV_SEQUENCE, count=10, base_params=[
# Parameter(name='auv_z_target',
# value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=-1.5))
# ], filter_params=[
# Parameter(name='four_dof',
# value=ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=False))
# ]),
#
# Experiment(note='four DoF filter', mode=Control.AUV_SEQUENCE, count=10, base_params=[
# Parameter(name='auv_z_target',
# value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=-1.5))
# ], filter_params=[
# Parameter(name='four_dof',
# value=ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=True))
# ]),
]
# self._experiments = []
# for step in range(1, 20, 1):
# self._experiments.append(Experiment(note='', mode=Control.AUV_SEQUENCE, count=1, base_params=[
# Parameter(name='auv_z_target',
# value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=-(2. + step/10.)))
# ], filter_params=[]))
# If we're in a mission record odometry and ground truth messages for later processing
self._odom_msgs: Optional[List[Odometry]] = None
self._gt_msgs: Optional[List[Odometry]] = None
self._odom_sub = self.create_subscription(Odometry, '/odom',
self.odom_cb, 5)
self._gt_sub = self.create_subscription(Odometry, '/ground_truth',
self.gt_cb, 5)
# Set parameter service clients
self._set_param_base_node_client = self.create_client(
SetParameters, '/base_node/set_parameters')
self._set_param_filter_node_client = self.create_client(
SetParameters, '/filter_node/set_parameters')
# Mission action client
self._mission_action_client = ActionClient(self, Mission, '/mission')
# Futures manage the async state changes
self._set_base_node_params_future = None
self._set_filter_node_params_future = None
self._send_goal_future = None
self._get_result_future = None
# Start 1st experiment
self._idx = 0
self._count = 0
self.get_logger().info(
'starting experiment {}, ({}), will run {} time(s)'.format(
self._idx, self._experiments[self._idx].note,
self._experiments[self._idx].count))
self.get_logger().info('starting run {} of {}'.format(
self._count + 1, self._experiments[self._idx].count))
# Step 1
self.set_base_node_params()
def dict_to_obj(d: Dict):
return [Parameter(name=item[0],
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE,
double_value=item[1]))
for item in d.items()]
return pid_param_obj(dim, [0.5, 0.0, 0.0])
# Get zn_no_overshoot PID parameters
# Not used in ft3
def zn_no_overshoot(Ku, Tu):
return [0.2 * Ku, 0.4 * Ku / Tu, 0.0667 * Ku * Tu]
def zn_no_overshoot_params(dim: str, Ku, Tu):
return pid_param_obj(dim, zn_no_overshoot(Ku, Tu))
base_auv_params = [
# Never replan
Parameter(name='global_plan_max_xy_err',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=10.0)),
# Trust markers up to 3m away
Parameter(name='good_pose_dist',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=3.0)),
]
# Normal motion, with local planning
# Use the planned yaw to rotate body to world frame (this is the default)
normal_motion_params = [
Parameter(name='pose_plan_pause_duration',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=1.0)),
Parameter(name='pose_plan_epsilon_xyz',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=0.05)),
Parameter(name='pose_plan_epsilon_yaw',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=0.05)),
from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue
import rclpy
import rclpy.logging
# Run velocities
auv_xy_velo = 0.4
auv_z_velo = 0.2 # Run slower to actually hit the run phase
auv_yaw_velo = 0.4
# Use these parameters to force a simple back/forth motion
straight_motion_auv_params = [
# pose_plan_max_short_plan_xy controls how much distance to cover with an "all DoF" motion,
# vs. the more typical "long plan" that turns toward the motion, runs, then turns again.
# Set this to a large value so that we can run backwards and side-to-side without turning.
Parameter(name='pose_plan_max_short_plan_xy',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE,
double_value=99.)),
# Turn off replanning
Parameter(name='global_plan_max_xy_err',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE,
double_value=99.0)),
# good_pose_dist controls how close we need to be to a marker to trust the pose.
# Set this to a large-ish value so that we always build a local plan at each pose.
Parameter(name='good_pose_dist',
value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE,
double_value=4.)),
# Specify the run velocities
Parameter(name='auv_xy_velo',
names_values[name] = value
line = f"{name:{l}s}\t{t_str}\t{v}"
parameters_prompt.append(line)
req_params = select_bullet("Select desired parameter", parameters_prompt)
param_name = req_params.split()[0]
p = f"Enter new value for parameter '{param_name}' of type {names_types[param_name]} "
new_value_str = get_string(p)
valid, new_value = is_value_param_valid(new_value_str,
names_values[param_name])
# print(valid, new_value_str, new_value)
if valid:
print(
f"You've entered '{new_value_str}'' which translates to '{new_value}'"
)
req = SetParameters.Request()
param = Parameter()
param.name = param_name
param.value = get_parameter_value(names_values[param_name].type,
new_value)
# print("---------")
# print(param.value)
# print("---------")
req.parameters.append(param)
f = set_params_client.call_async(req)
req_time = time.time()
while not f.done():
rclpy.spin_once(node, timeout_sec=0.1)
if time.time() - req_time > 3.0:
print("Timedout requesting set_parameters")
break
if f.done():
