def _load(self, node: Node):
load_complete = True
parameters = self.__dict__
for name, value in parameters.items():
if name in ['robot_name', 'fleet_name']:
continue
try:
node.declare_parameter(name)
except ParameterAlreadyDeclaredException:
pass
Utils.logger.info(f'Loading {name} of type {type(value)}')
if type(value) is str:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().string_value
elif type(value) is bool:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().bool_value
elif type(value) is float:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().double_value
if not self.__dict__[name] and type(value) is not bool:
load_complete = False
if not load_complete:
Utils.logger.info('Parameters not yet fully loaded. Reloading..')
time.sleep(2)
self._load(node)
def __init__(self, node:Node):
self.node = node
self.shut_down_request = False
self.last_initialized = None
self.initialized = False
self.use_sim_time = self.node.get_parameter('use_sim_time').value
# we only need tf in simulation. don't use it otherwise to safe performance
if self.use_sim_time:
self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=10))
self.tf_listener = tf2.TransformListener(self.tf_buffer, node)
self.odom_frame = node.get_parameter('odom_frame').get_parameter_value().string_value
self.base_footprint_frame = node.get_parameter('base_footprint_frame').get_parameter_value().string_value
self.field_length = node.get_parameter('field_length').get_parameter_value().double_value
# services
self.reset_filter_proxy = node.create_client(ResetFilter, 'reset_localization')
self.stop_filter_proxy = node.create_client(SetPaused, 'pause_localization')
# Pose
self.last_pose_update_time = None
self.poseX = 0
self.poseY = 0
self.orientation = np.array([0, 0, 0, 1])
self.covariance = np.array([])
#GameState
self.gamestate = GameStatusCapsule(node)
#Robot Control State
self.robot_control_state = None
self.last_robot_control_state = None
#Get up
self.last_state_get_up = False
#Picked up
self.last_state_pickup = False
#Last init action
self.last_init_action_type = False
self.last_init_odom_transform = None
def __init__(self, node:Node):
node.declare_parameter("robot_type", "wolfgang")
anim_package = node.get_parameter('robot_type').get_parameter_value().string_value + "_animations"
path = get_package_share_directory(anim_package)
self.basepath = abspath(path + "/animations")
self.cache = {}
self.files = [] # Animations cached for find_all_animations
self.names = [] # Plain animation names, without filename-extension
self.animpath = self._get_animpath()
def main():
rclpy.init()
node = Node('node',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
agent_id = node.get_parameter('agent_id').value
turtlebot3_position_control = Turtlebot3Feedback(agent_id, 'pubsub',
'odom')
rclpy.spin(turtlebot3_position_control)
turtlebot3_position_control.destroy_node()
rclpy.shutdown()
def main():
rclpy.init()
node = Node('table_parameters',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
N = node.get_parameter('N').value
table = PositionTaskTable(N)
table.gc.trigger()
table.get_logger().info('Waiting for 10 seconds to let all nodes be ready')
time.sleep(10)
rclpy.spin(table)
rclpy.shutdown()
def generate(args=None):
rclpy.init(args=args)
# Node name should be declared
node = Node('generate')
# Even if you pass parameters via command line by using yaml file,
# should declare parameter name
node.declare_parameter(name='config_path', value='')
config_path = node.get_parameter('config_path').value
try:
factory(config_path)
except Exception as e:
# In python3 Exception has no attribute 'message'
print(e)
exit(-1)
exit(0)
def view_taylor(args=None):
rclpy.init(args=args)
node = Node('view_taylor')
node.declare_parameter(name='config_path', value='')
config_path = node.get_parameter('config_path').value
f = open(config_path, 'r')
config_dict = yaml.load(f, Loader=yaml.FullLoader)
model = models.create(config_dict['model'])
contex = taylor_expansion_context.create(config_dict['context'], model)
contex.run_expansion()
try:
input('press enter to terminate')
except:
pass
exit(0)
class HardwareControlManager:
def __init__(self):
rclpy.init(args=None)
self.node = Node("hcm",
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
multi_executor = MultiThreadedExecutor()
multi_executor.add_node(self.node)
self.spin_thread = threading.Thread(target=multi_executor.spin,
args=(),
daemon=True)
#self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
self.spin_thread.start()
self.blackboard = None
# --- Initialize Node ---
# Otherwise messages will get lost, bc the init is not finished
self.node.get_clock().sleep_for(Duration(seconds=0.1))
self.node.get_logger().debug("Starting hcm")
self.simulation_active = self.node.get_parameter("simulation_active")
# dsd
self.blackboard = HcmBlackboard(self.node)
self.blackboard.animation_action_client = ActionClient(
self.node, PlayAnimation, 'animation')
self.blackboard.dynup_action_client = ActionClient(
self.node, Dynup, 'dynup')
dirname = os.path.dirname(os.path.realpath(__file__)) + "/hcm_dsd"
self.dsd = DSD(self.blackboard, "debug/dsd/hcm", node=self.node)
self.dsd.register_actions(os.path.join(dirname, 'actions'))
self.dsd.register_decisions(os.path.join(dirname, 'decisions'))
self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd'))
self.hcm_deactivated = False
# Publisher / subscriber
self.joint_goal_publisher = self.node.create_publisher(
JointCommand, 'DynamixelController/command', 1)
self.hcm_state_publisher = self.node.create_publisher(
RobotControlState, 'robot_state', 1) # todo latch
self.blackboard.speak_publisher = self.node.create_publisher(
Audio, 'speak', 1)
# important to make sure the connection to the speaker is established, for next line
self.node.get_clock().sleep_for(Duration(seconds=0.1))
speak("Starting hcm", self.blackboard.speak_publisher, priority=50)
self.node.create_subscription(Imu, "imu/data", self.update_imu, 1)
self.node.create_subscription(FootPressure,
"foot_pressure_left/filtered",
self.update_pressure_left, 1)
self.node.create_subscription(FootPressure,
"foot_pressure_right/filtered",
self.update_pressure_right, 1)
self.node.create_subscription(JointCommand, "walking_motor_goals",
self.walking_goal_callback, 1)
self.node.create_subscription(AnimationMsg, "animation",
self.animation_callback, 1)
self.node.create_subscription(JointCommand, "dynup_motor_goals",
self.dynup_callback, 1)
self.node.create_subscription(JointCommand, "head_motor_goals",
self.head_goal_callback, 1)
self.node.create_subscription(JointCommand, "record_motor_goals",
self.record_goal_callback, 1)
self.node.create_subscription(JointCommand, "kick_motor_goals",
self.kick_goal_callback, 1)
self.node.create_subscription(Bool, "pause", self.pause, 1)
self.node.create_subscription(JointState, "joint_states",
self.joint_state_callback, 1)
self.node.create_subscription(PointStamped, "cop_l", self.cop_l_cb, 1)
self.node.create_subscription(PointStamped, "cop_r", self.cop_r_cb, 1)
self.node.create_subscription(Bool, "core/power_switch_status",
self.power_cb, 1)
self.node.create_subscription(Bool, "hcm_deactivate",
self.deactivate_cb, 1)
self.node.create_subscription(DiagnosticArray, "diagnostics_agg",
self.blackboard.diag_cb, 1)
self.node.add_on_set_parameters_callback(self.on_set_parameters)
self.main_loop()
def deactivate_cb(self, msg):
self.hcm_deactivated = msg.data
def pause(self, msg):
""" Updates the stop state for the state machine"""
self.blackboard.stopped = msg.data
def update_imu(self, msg):
"""Gets new IMU values and computes the smoothed values of these"""
self.blackboard.last_imu_update_time = msg.header.stamp
self.blackboard.accel = numpy.array([
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
])
self.blackboard.gyro = numpy.array([
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z
])
self.blackboard.quaternion = numpy.array(([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]))
self.blackboard.smooth_gyro = numpy.multiply(
self.blackboard.smooth_gyro, 0.95) + numpy.multiply(
self.blackboard.gyro, 0.05)
self.blackboard.smooth_accel = numpy.multiply(
self.blackboard.smooth_accel, 0.99) + numpy.multiply(
self.blackboard.accel, 0.01)
self.blackboard.not_much_smoothed_gyro = numpy.multiply(
self.blackboard.not_much_smoothed_gyro, 0.5) + numpy.multiply(
self.blackboard.gyro, 0.5)
self.blackboard.imu_msg = msg
def update_pressure_left(self, msg):
"""Gets new pressure values and writes them to the blackboard"""
self.blackboard.last_pressure_update_time = msg.header.stamp
self.blackboard.pressures[0] = msg.left_front
self.blackboard.pressures[1] = msg.left_back
self.blackboard.pressures[2] = msg.right_front
self.blackboard.pressures[3] = msg.right_back
def update_pressure_right(self, msg):
"""Gets new pressure values and writes them to the blackboard"""
self.blackboard.last_pressure_update_time = msg.header.stamp
self.blackboard.pressures[4] = msg.left_front
self.blackboard.pressures[5] = msg.left_back
self.blackboard.pressures[6] = msg.right_front
self.blackboard.pressures[7] = msg.right_back
def on_set_parameters(self, config, level):
""" Dynamic reconfigure of the fall checker values."""
# just pass on to the StandupHandler, as all the variables are located there
self.blackboard.fall_checker.update_reconfigurable_values(
config, level)
self.blackboard.pickup_accel_threshold = config[
"pick_up_accel_threshold"]
return config
def walking_goal_callback(self, msg):
self.blackboard.last_walking_goal_time = self.node.get_clock().now()
if self.blackboard.current_state in [
RobotControlState.CONTROLLABLE, RobotControlState.WALKING
]:
self.joint_goal_publisher.publish(msg)
def dynup_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.STARTUP, RobotControlState.FALLEN,
RobotControlState.GETTING_UP, RobotControlState.CONTROLLABLE
]:
self.joint_goal_publisher.publish(msg)
def head_goal_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.CONTROLLABLE, RobotControlState.WALKING
]:
# we can move our head
self.joint_goal_publisher.publish(msg)
def record_goal_callback(self, msg):
if msg.joint_names == []:
# record tells us that its finished
self.blackboard.record_active = False
else:
self.blackboard.record_active = True
self.joint_goal_publisher.publish(msg)
def kick_goal_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.KICKING, RobotControlState.CONTROLLABLE
]:
# we can perform a kick
self.joint_goal_publisher.publish(msg)
def animation_callback(self, msg):
""" The animation server is sending us goal positions for the next keyframe"""
self.blackboard.last_animation_goal_time = msg.header.stamp
if msg.request:
self.node.get_logger().info(
"Got Animation request. HCM will try to get controllable now.")
# animation has to wait
# dsd should try to become controllable
self.blackboard.animation_requested = True
return
if msg.first:
if msg.hcm:
# coming from ourselves
# we don't have to do anything, since we must be in the right state
pass
else:
# coming from outside
# check if we can run an animation now
if self.blackboard.current_state != RobotControlState.CONTROLLABLE:
self.node.get_logger().warn(
"HCM is not controllable, animation refused.")
return
else:
# we're already controllable, go to animation running
self.blackboard.external_animation_running = True
if msg.last:
if msg.hcm:
# This was an animation from the DSD
self.blackboard.hcm_animation_finished = True
pass
else:
# this is the last frame, we want to tell the DSD that we're finished with the animations
self.blackboard.external_animation_running = False
if msg.position is None:
# probably this was just to tell us we're finished
# we don't need to set another position to the motors
return
# forward positions to motors, if some where transmitted
if len(
msg.position.points
) > 0 and self.blackboard.current_state != RobotControlState.GETTING_UP:
out_msg = JointCommand()
out_msg.positions = msg.position.points[0].positions
out_msg.joint_names = msg.position.joint_names
out_msg.accelerations = [-1.0] * len(out_msg.joint_names)
out_msg.velocities = [-1.0] * len(out_msg.joint_names)
out_msg.max_currents = [-1.0] * len(out_msg.joint_names)
if msg.position.points[0].effort:
out_msg.max_currents = [
-x for x in msg.position.points[0].effort
]
if self.blackboard.shut_down_request:
# there are sometimes transmission errors during shutdown due to race conditions
# there is nothing we can do so just ignore the errors in this case
try:
self.joint_goal_publisher.publish(out_msg)
except:
pass
else:
self.joint_goal_publisher.publish(out_msg)
def joint_state_callback(self, msg: JointState):
self.blackboard.last_motor_update_time = msg.header.stamp
self.blackboard.previous_joint_state = self.blackboard.current_joint_state
self.blackboard.current_joint_state = msg
def cop_l_cb(self, msg):
self.blackboard.cop_l_msg = msg
def cop_r_cb(self, msg):
self.blackboard.cop_r_msg = msg
def power_cb(self, msg):
self.blackboard.is_power_on = msg.data
def main_loop(self):
""" Keeps updating the DSD and publish its current state.
All the forwarding of joint goals is directly done in the callbacks to reduce latency. """
last_loop_start_time = self.node.get_clock().now()
while rclpy.ok() and not self.blackboard.shut_down_request:
try:
loop_start_time = self.node.get_clock().now()
#can happen in simulation due to bad implementation in rclpy
if (last_loop_start_time != loop_start_time):
last_loop_start_time = loop_start_time
if self.hcm_deactivated:
self.blackboard.current_state = RobotControlState.CONTROLLABLE
msg = RobotControlState()
msg.state = self.blackboard.current_state
self.hcm_state_publisher.publish(msg)
else:
self.blackboard.current_time = self.node.get_clock(
).now()
try:
self.dsd.update()
msg = RobotControlState()
msg.state = self.blackboard.current_state
self.hcm_state_publisher.publish(msg)
except IndexError:
# this error will happen during shutdown procedure, just ignore it
pass
self.node.get_clock().sleep_until(loop_start_time +
Duration(seconds=1 / 500.0))
except (ExternalShutdownException, KeyboardInterrupt):
if not self.simulation_active:
self.on_shutdown_hook()
exit(0)
if not self.simulation_active:
self.on_shutdown_hook()
self.spin_thread.join()
def on_shutdown_hook(self):
if not self.blackboard:
return
# we got external shutdown, tell it to the DSD, it will handle it
self.blackboard.shut_down_request = True
self.node.get_logger().warn(
"You're stopping the HCM. The robot will sit down and power off its motors."
)
speak("Stopping HCM", self.blackboard.speak_publisher, priority=50)
# now wait for it finishing the shutdown procedure
while not self.blackboard.current_state == RobotControlState.HCM_OFF:
# we still have to update everything
self.blackboard.current_time = self.node.get_clock().now()
self.dsd.update()
self.hcm_state_publisher.publish(self.blackboard.current_state)
self.node.get_clock().sleep_for(Duration(seconds=0.01))
def main():
rclpy.init(args=None)
node = Node('system_monitor')
pub = node.create_publisher(WorkloadMsg, 'system_workload', 1)
diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1)
hostname = socket.gethostname()
diag_array = DiagnosticArray()
diag_cpu = DiagnosticStatus()
# start all names with "SYSTEM" for diagnostic analysesr
diag_cpu.name = "SYSTEMCPU"
diag_cpu.hardware_id = "CPU"
diag_mem = DiagnosticStatus()
diag_mem.name = "SYSTEMMemory"
diag_cpu.hardware_id = "Memory"
node.declare_parameter('update_frequency', 10.0)
node.declare_parameter('do_memory', True)
node.declare_parameter('do_cpu', True)
node.declare_parameter('cpu_load_percentage', 80.0)
node.declare_parameter('memoroy_load_percentage', 80.0)
node.declare_parameter('network_rate_received_errors', 10.0)
node.declare_parameter('network_rate_send_errors', 10.0)
rate = node.get_parameter('update_frequency').get_parameter_value().double_value
do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value
do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value
cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value
memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value
network_rate_received_errors = node.get_parameter(
'network_rate_received_errors').get_parameter_value().double_value
network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value
while rclpy.ok():
last_send_time = time.time()
running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (
-1, [], 0)
memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1)
interfaces = network_interfaces.collect_all(node.get_clock())
msg = WorkloadMsg(
hostname=hostname,
cpus=cpu_usages,
running_processes=running_processes,
cpu_usage_overall=overall_usage_percentage,
memory_available=memory_available,
memory_used=memory_used,
memory_total=memory_total,
filesystems=[],
network_interfaces=interfaces
)
pub.publish(msg)
diag_array.status = []
# publish diagnostic message to show this in the rqt diagnostic viewer
diag_cpu.message = str(overall_usage_percentage) + "%"
if overall_usage_percentage >= cpu_load_percentage:
diag_cpu.level = DiagnosticStatus.WARN
else:
diag_cpu.level = DiagnosticStatus.OK
diag_array.status.append(diag_cpu)
memory_usage = round((memory_used / memory_total) * 100, 2)
diag_mem.message = str(memory_usage) + "%"
if memory_usage >= memoroy_load_percentage:
diag_mem.level = DiagnosticStatus.WARN
else:
diag_mem.level = DiagnosticStatus.OK
diag_array.status.append(diag_mem)
for interface in interfaces:
diag_net = DiagnosticStatus()
diag_net.name = "SYSTEM" + interface.name
diag_net.hardware_id = interface.name
if interface.rate_received_packets_errors >= network_rate_received_errors \
or interface.rate_sent_packets_errors >= network_rate_send_errors:
diag_net.level = DiagnosticStatus.WARN
else:
diag_net.level = DiagnosticStatus.OK
diag_array.status.append(diag_net)
diag_array.header.stamp = node.get_clock().now().to_msg()
diagnostic_pub.publish(diag_array)
# sleep to have correct rate. we dont use ROS time since we are interested in system things
dt = time.time() - last_send_time
time.sleep(max(0, (1 / rate) - dt))
def optimize(args=None):
rclpy.init(args=args)
node = Node('optimize')
node.declare_parameter(name='opt_config_path', value='')
node.declare_parameter(name='data_config_path', value='')
# データ
data_config_path = node.get_parameter('data_config_path').value
data = generate(data_config_path, as_node=True)
opt_config_path = node.get_parameter('opt_config_path').value
with open(opt_config_path, 'r') as f:
opt_config_dict = yaml.load(f, Loader=yaml.FullLoader)
# モデル
model = models.create(opt_config_dict['model'])
# パラメータ更新器
updater = updaters.create(opt_config_dict['updater'])
# 最適化処理クラス
optimizer = optimizers.create(model, data.obs, updater,
opt_config_dict['optimizer'])
# 描画クラス(optional)
plotter = None
if 'plotter' in opt_config_dict and opt_config_dict['plotter']:
plotter = plotters.create(opt_config_dict['plotter'], model, data.obs)
# 1周期毎に停止(optional)
once = False
if 'once' in opt_config_dict and opt_config_dict['once']:
once = opt_config_dict['once']
print('initial param : {}'.format(model.get_param()))
print('initial error of sum squares: {}'.format(optimizer.ess()))
print('Optimize...')
previous_num = 0
num_iteration = 0
while True:
if plotter:
print('Plotting...')
plotter.plot()
if once:
try:
print('press enter to next iteration')
input()
except:
pass
num_iteration = optimizer.optimize(once=once)
if previous_num == num_iteration:
break
previous_num = num_iteration
print('num of iteration : {}'.format(num_iteration))
print('current param : {}'.format(model.get_param()))
print('current error of sum squares: {}'.format(optimizer.ess()))
try:
input('\npress enter to terminate')
except:
pass
exit(0)
def _load(self, node: Node, retries=5):
if retries == 0:
raise ValueError('Failed to load parameters.')
time.sleep(0.1) # Throttle so we don't run out of stack space
load_complete = True
params = self.__dict__
# fixed parameters are those defined in init function
fixed_params = list(inspect.signature(self.__init__).parameters.keys())
for name, param_type in params.items():
# If provided in __init__, do not load
if name in fixed_params:
continue
# Declare parameter if it is new
try:
node.declare_parameter(name)
except ParameterAlreadyDeclaredException:
pass
Utils.logger.info(
f'Loading ROS2 parameter {name} of type {param_type}')
# Load the appropriate value
if param_type is str:
param = node.get_parameter(
name).get_parameter_value().string_value
if param:
self.__dict__[name] = param
elif param_type is bool:
param = node.get_parameter(
name).get_parameter_value().bool_value
self.__dict__[name] = param
elif param_type is float:
param = node.get_parameter(
name).get_parameter_value().double_value
if param:
self.__dict__[name] = param
elif param_type is int:
self.__dict__[name] = node.get_parameter(
name).get_parameter_value().integer_value
if param:
self.__dict__[name] = param
if type(self.__dict__[name]) is not type:
Utils.logger.info(f'{name} has value {self.__dict__[name]}')
else:
Utils.logger.info(f'{name} was not loaded.')
param = None
if any(map(lambda x: type(x) is type, self.__dict__.values())):
load_complete = False
if not load_complete:
Utils.logger.info('Parameters not yet fully loaded. Reloading..')
time.sleep(1) # Throttle so we don't run out of stack space
self._load(node, retries=(retries - 1))
else:
Utils.logger.info(f'{self.__class__.__name__} parameters: ')
Utils.logger.info(f'{self}')
from __future__ import print_function
import rclpy
from rcply.duration import Duration
from rclpy.node import Node
import tf2_ros as tf2
from bio_ik_msgs.msg import IKRequest, LookAtGoal
from bio_ik_msgs.srv import GetIK
from bitbots_msgs.msg import JointCommand
from geometry_msgs.msg import Point
from time import sleep
from tf2_geometry_msgs import PointStamped
if __name__ == "__main__":
rclpy.init(args=None)
node = Node("test_look_at")
base_footprint_frame = node.get_parameter(
'~base_footprint_frame').get_parameter_value().double_value
camera_frame = node.get_parameter(
'~camera_frame').get_parameter_value().double_value
base_link_frame = node.get_parameter(
'~base_link_frame').get_parameter_value().double_value
head_tf_frame = base_link_frame
tf_buffer = tf2.Buffer(Duration(seconds=5))
tf_listener = tf2.TransformListener(tf_buffer)
request = IKRequest()
request.group_name = "Head"
request.timeout.secs = 1
request.approximate = True
request.look_at_goals.append(LookAtGoal())
request.look_at_goals[0].link_name = camera_frame
def __init__(self, node: Node):
node.get_logger().info('Initializing parameters')
# Declare parameters of the ROS2 node and their default values:
# The topic prefix to use (can be empty if not required)
node.declare_parameter(name='ros_topic_prefix', value='bno055/')
# The type of the sensor connection. Either "uart" or "i2c":
node.declare_parameter(name='connection_type',
value=UART.CONNECTIONTYPE_UART)
# UART port
node.declare_parameter('uart_port', value='/dev/ttyUSB0')
# UART Baud Rate
node.declare_parameter('uart_baudrate', value=115200)
# UART Timeout in seconds
node.declare_parameter('uart_timeout', value=0.1)
# tf frame id
node.declare_parameter('frame_id', value='bno055')
# Node timer frequency in Hz, defining how often sensor data is requested
node.declare_parameter('data_query_frequency', value=10)
# Node timer frequency in Hz, defining how often calibration status data is requested
node.declare_parameter('calib_status_frequency', value=0.1)
# sensor operation mode
node.declare_parameter('operation_mode', value=0x0C)
# placement_axis_remap defines the position and orientation of the sensor mount
node.declare_parameter('placement_axis_remap', value='P1')
# +/- 2000 units (at max 2G) (1 unit = 1 mg = 1 LSB = 0.01 m/s2)
node.declare_parameter('acc_offset', value=[0xFFEC, 0x00A5, 0xFFE8])
# +/- 6400 units (1 unit = 1/16 uT)
node.declare_parameter('mag_offset', value=[0xFFB4, 0xFE9E, 0x027D])
# +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps)
node.declare_parameter('gyr_offset', value=[0x0002, 0xFFFF, 0xFFFF])
# get the parameters - requires CLI arguments '--ros-args --params-file <parameter file>'
node.get_logger().info('Parameters set to:')
try:
self.ros_topic_prefix = node.get_parameter('ros_topic_prefix')
node.get_logger().info('\tros_topic_prefix:\t"%s"' %
self.ros_topic_prefix.value)
self.connection_type = node.get_parameter('connection_type')
node.get_logger().info('\tconnection_type:\t"%s"' %
self.connection_type.value)
self.uart_port = node.get_parameter('uart_port')
node.get_logger().info('\tuart_port:\t\t"%s"' %
self.uart_port.value)
self.uart_baudrate = node.get_parameter('uart_baudrate')
node.get_logger().info('\tuart_baudrate:\t\t"%s"' %
self.uart_baudrate.value)
self.uart_timeout = node.get_parameter('uart_timeout')
node.get_logger().info('\tuart_timeout:\t\t"%s"' %
self.uart_timeout.value)
self.frame_id = node.get_parameter('frame_id')
node.get_logger().info('\tframe_id:\t\t"%s"' % self.frame_id.value)
self.data_query_frequency = node.get_parameter(
'data_query_frequency')
node.get_logger().info('\tdata_query_frequency:\t"%s"' %
self.data_query_frequency.value)
self.calib_status_frequency = node.get_parameter(
'calib_status_frequency')
node.get_logger().info('\tcalib_status_frequency:\t"%s"' %
self.calib_status_frequency.value)
self.operation_mode = node.get_parameter('operation_mode')
node.get_logger().info('\toperation_mode:\t\t"%s"' %
self.operation_mode.value)
self.placement_axis_remap = node.get_parameter(
'placement_axis_remap')
node.get_logger().info('\tplacement_axis_remap:\t"%s"' %
self.placement_axis_remap.value)
self.acc_offset = node.get_parameter('acc_offset')
node.get_logger().info('\tacc_offset:\t\t"%s"' %
self.acc_offset.value)
self.mag_offset = node.get_parameter('mag_offset')
node.get_logger().info('\tmag_offset:\t\t"%s"' %
self.mag_offset.value)
self.gyr_offset = node.get_parameter('gyr_offset')
node.get_logger().info('\tgyr_offset:\t\t"%s"' %
self.gyr_offset.value)
except Exception as e: # noqa: B902
node.get_logger().warn(
'Could not get parameters...setting variables to default')
node.get_logger().warn('Error: "%s"' % e)
def __init__(self, node: Node):
node.get_logger().info('Initializing parameters')
# Declare parameters of the ROS2 node and their default values:
# The topic prefix to use (can be empty if not required)
node.declare_parameter(name='ros_topic_prefix', value='bno055/')
# The type of the sensor connection. Either "uart" or "i2c":
node.declare_parameter(name='connection_type',
value=UART.CONNECTIONTYPE_UART)
# UART port
node.declare_parameter('uart_port', value='/dev/ttyUSB0')
# UART Baud Rate
node.declare_parameter('uart_baudrate', value=115200)
# UART Timeout in seconds
node.declare_parameter('uart_timeout', value=0.1)
# tf frame id
node.declare_parameter('frame_id', value='bno055')
# Node timer frequency in Hz, defining how often sensor data is requested
node.declare_parameter('data_query_frequency', value=10)
# Node timer frequency in Hz, defining how often calibration status data is requested
node.declare_parameter('calib_status_frequency', value=0.1)
# sensor operation mode
node.declare_parameter('operation_mode', value=0x0C)
# placement_axis_remap defines the position and orientation of the sensor mount
node.declare_parameter('placement_axis_remap', value='P1')
# scaling factor for acceleration
node.declare_parameter('acc_factor', value=100.0)
# scaling factor for magnetometer
node.declare_parameter('mag_factor', value=16000000.0)
# scaling factor for gyroscope
node.declare_parameter('gyr_factor', value=900.0)
# determines whether to use default offsets or not
node.declare_parameter('set_offsets', value=False)
# +/- 2000 units (at max 2G) (1 unit = 1 mg = 1 LSB = 0.01 m/s2)
node.declare_parameter('offset_acc',
value=registers.DEFAULT_OFFSET_ACC)
# +/- 6400 units (1 unit = 1/16 uT)
node.declare_parameter('offset_mag',
value=registers.DEFAULT_OFFSET_MAG)
# +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps)
node.declare_parameter('offset_gyr',
value=registers.DEFAULT_OFFSET_GYR)
# +/-1000 units
node.declare_parameter('radius_acc',
value=registers.DEFAULT_RADIUS_ACC)
# +/-960 units
node.declare_parameter('radius_mag',
value=registers.DEFAULT_RADIUS_MAG)
# Sensor standard deviation squared (^2) defaults [x, y, z]
node.declare_parameter('variance_acc',
value=registers.DEFAULT_VARIANCE_ACC)
node.declare_parameter('variance_angular_vel',
value=registers.DEFAULT_VARIANCE_ANGULAR_VEL)
node.declare_parameter('variance_orientation',
value=registers.DEFAULT_VARIANCE_ORIENTATION)
node.declare_parameter('variance_mag',
value=registers.DEFAULT_VARIANCE_MAG)
# get the parameters - requires CLI arguments '--ros-args --params-file <parameter file>'
node.get_logger().info('Parameters set to:')
try:
self.ros_topic_prefix = node.get_parameter('ros_topic_prefix')
node.get_logger().info('\tros_topic_prefix:\t"%s"' %
self.ros_topic_prefix.value)
self.connection_type = node.get_parameter('connection_type')
node.get_logger().info('\tconnection_type:\t"%s"' %
self.connection_type.value)
self.uart_port = node.get_parameter('uart_port')
node.get_logger().info('\tuart_port:\t\t"%s"' %
self.uart_port.value)
self.uart_baudrate = node.get_parameter('uart_baudrate')
node.get_logger().info('\tuart_baudrate:\t\t"%s"' %
self.uart_baudrate.value)
self.uart_timeout = node.get_parameter('uart_timeout')
node.get_logger().info('\tuart_timeout:\t\t"%s"' %
self.uart_timeout.value)
self.frame_id = node.get_parameter('frame_id')
node.get_logger().info('\tframe_id:\t\t"%s"' % self.frame_id.value)
self.data_query_frequency = node.get_parameter(
'data_query_frequency')
node.get_logger().info('\tdata_query_frequency:\t"%s"' %
self.data_query_frequency.value)
self.calib_status_frequency = node.get_parameter(
'calib_status_frequency')
node.get_logger().info('\tcalib_status_frequency:\t"%s"' %
self.calib_status_frequency.value)
self.operation_mode = node.get_parameter('operation_mode')
node.get_logger().info('\toperation_mode:\t\t"%s"' %
self.operation_mode.value)
self.placement_axis_remap = node.get_parameter(
'placement_axis_remap')
node.get_logger().info('\tplacement_axis_remap:\t"%s"' %
self.placement_axis_remap.value)
self.acc_factor = node.get_parameter('acc_factor')
node.get_logger().info('\tacc_factor:\t\t"%s"' %
self.acc_factor.value)
self.mag_factor = node.get_parameter('mag_factor')
node.get_logger().info('\tmag_factor:\t\t"%s"' %
self.mag_factor.value)
self.gyr_factor = node.get_parameter('gyr_factor')
node.get_logger().info('\tgyr_factor:\t\t"%s"' %
self.gyr_factor.value)
self.set_offsets = node.get_parameter('set_offsets')
node.get_logger().info('\tset_offsets:\t\t"%s"' %
self.set_offsets.value)
self.offset_acc = node.get_parameter('offset_acc')
node.get_logger().info('\toffset_acc:\t\t"%s"' %
self.offset_acc.value)
self.radius_acc = node.get_parameter('radius_acc')
node.get_logger().info('\tradius_acc:\t\t"%s"' %
self.radius_acc.value)
self.offset_mag = node.get_parameter('offset_mag')
node.get_logger().info('\toffset_mag:\t\t"%s"' %
self.offset_mag.value)
self.radius_mag = node.get_parameter('radius_mag')
node.get_logger().info('\tradius_mag:\t\t"%s"' %
self.radius_mag.value)
self.offset_gyr = node.get_parameter('offset_gyr')
node.get_logger().info('\toffset_gyr:\t\t"%s"' %
self.offset_gyr.value)
self.variance_acc = node.get_parameter('variance_acc')
node.get_logger().info('\tvariance_acc:\t\t"%s"' %
self.variance_acc.value)
self.variance_angular_vel = node.get_parameter(
'variance_angular_vel')
node.get_logger().info('\tvariance_angular_vel:\t"%s"' %
self.variance_angular_vel.value)
self.variance_orientation = node.get_parameter(
'variance_orientation')
node.get_logger().info('\tvariance_orientation:\t"%s"' %
self.variance_orientation.value)
self.variance_mag = node.get_parameter('variance_mag')
node.get_logger().info('\tvariance_mag:\t\t"%s"' %
self.variance_mag.value)
except Exception as e: # noqa: B902
node.get_logger().warn(
'Could not get parameters...setting variables to default')
node.get_logger().warn('Error: "%s"' % e)
