def to_path_marker(self, node: Node, clear=False):
"""Return a `nav_msgs/Path` message with all waypoints in the set
> *Input arguments*
* `clear` (*type:* `bool`, *default:* `False`): Return an empty `nav_msgs/Path` message.
> *Returns*
`nav_msgs/Path` message
"""
path = Path()
t = node.get_clock().now()
path.header.stamp = t.to_msg()
path.header.frame_id = self._inertial_frame_id
if self.num_waypoints > 1 and not clear:
for i in range(self.num_waypoints):
wp = self.get_waypoint(i)
pose = PoseStamped()
pose.header.stamp = rclpy.time.Time(seconds=i) # TODO check
pose.header.frame_id = self._inertial_frame_id
pose.pose.position.x = wp.x
pose.pose.position.y = wp.y
pose.pose.position.z = wp.z
path.poses.append(pose)
return path
def timer_callback(self):
"""Timer Callback Function
This method captures images and publishes required data in ros 2 topic.
"""
if self.capture.isOpened():
# reads image data
ret, frame = self.capture.read()
# processes image data and converts to ros 2 message
msg = Image()
msg.header.stamp = Node.get_clock(self).now().to_msg()
msg.header.frame_id = 'ANI717'
msg.height = np.shape(frame)[0]
msg.width = np.shape(frame)[1]
msg.encoding = "bgr8"
msg.is_bigendian = False
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
msg.data = np.array(frame).tobytes()
# publishes message
self.publisher_.publish(msg)
self.get_logger().info('%d Images Published' % self.i)
# image counter increment
self.i += 1
return None
def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None:
# The logic for responding to this joystick press is:
# 1. Save off the current state of active.
# 2. Update the current state of active based on buttons and axes.
# 3. If this command is currently not active, return without publishing.
# 4. If this is a msg_value, and the value of the previous active is the same as now,
# debounce and return without publishing.
# 5. In all other cases, publish. This means that this is a msg_value and the button
# transitioned from 0 -> 1, or it means that this is an axis mapping and data should
# continue to be published without debouncing.
last_active = self.active
self.update_active_from_buttons_and_axes(joy_state)
if not self.active:
return
if self.msg_value is not None and last_active == self.active:
return
if self.msg_value is not None:
# This is the case for a static message.
msg = self.msg_value
else:
# This is the case to forward along mappings.
msg = self.topic_type()
for mapping, values in self.axis_mappings.items():
if 'axis' in values:
if len(joy_state.axes) > values['axis']:
val = joy_state.axes[values['axis']] * values.get('scale', 1.0) + \
values.get('offset', 0.0)
else:
node.get_logger().error('Joystick has only {} axes (indexed from 0),'
'but #{} was referenced in config.'.format(
len(joy_state.axes), values['axis']))
val = 0.0
elif 'button' in values:
if len(joy_state.buttons) > values['button']:
val = joy_state.buttons[values['button']] * values.get('scale', 1.0) + \
values.get('offset', 0.0)
else:
node.get_logger().error('Joystick has only {} buttons (indexed from 0),'
'but #{} was referenced in config.'.format(
len(joy_state.buttons), values['button']))
val = 0.0
elif 'value' in values:
# Pass on the value as its Python-implicit type
val = values.get('value')
else:
node.get_logger().error(
'No Supported axis_mappings type found in: {}'.format(mapping))
val = 0.0
set_member(msg, mapping, val)
# If there is a stamp field, fill it with now().
if hasattr(msg, 'header'):
msg.header.stamp = node.get_clock().now().to_msg()
self.pub.publish(msg)
def __init__(self,origion_x=0.0,origion_y=0.0,resolution=0.1, width=50, height=50):
super().__init__('map_pub')
self.msg = OccupancyGrid()
#self.pub = self.create_publisher(OccupancyGrid)
#timer_period = 0.05
#self.timer = self.create_timer(timer_period, self.scan)
self.time = Node.get_clock(self)
self.time = self.get_clock().now().to_msg()
self.orogion_x = origion_x
self.origion_y = origion_y
self.resolution = resolution
self.width = width
self.height = height
self.grid = np.zeros((height, width))
self.time = Node.get_clock(self)
self.time = self.get_clock().now().to_msg()
def __init__(self):
super().__init__('pose_estimate')
self.msg = geometry_msgs.msg.TransformStamped()
self.pub = self.create_publisher(geometry_msgs.msg.TransformStamped,
"/tf", 1)
timer_period = 0.05
self.timer = self.create_timer(timer_period, self.scan)
self.time = Node.get_clock(self)
self.time = self.get_clock().now().to_msg()
def __init__(self):
_node = Node("tf2twist")
qos_profile = QoSProfile(depth=10)
# loop_rate = _node.create_rate(100)
tfBuffer = Buffer()
listener = TransformListener(tfBuffer, _node, qos=qos_profile)
twist_data = geometry_msgs.msg.Twist()
pub_cmd_vel = _node.create_publisher(geometry_msgs.msg.Twist,
"cmd_vel", qos_profile)
# self.transfromstamped =
try:
while rclpy.ok():
rclpy.spin_once(_node)
now = _node.get_clock().now() - rclpy.duration.Duration(
seconds=0, nanoseconds=1000000)
try:
trans = tfBuffer.lookup_transform(
'odom_frame', 'base_link', now,
rclpy.duration.Duration(seconds=0, nanoseconds=0))
# print(trans)
# print(trans.transform.rotation.x)
roll, pitch, yaw = self.quaternion_to_euler(
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
# print(yaw)
if (yaw > 0.3):
twist_data.angular.z = 70.0 #yaw*125*1.3
elif (yaw < -0.3):
twist_data.angular.z = -70.0 #yaw*125*1.3
else:
twist_data.angular.z = 0.0
pub_cmd_vel.publish(twist_data)
except (LookupException, LookupError, ConnectionAbortedError,
ConnectionError, ConnectionRefusedError,
ConnectionResetError, ExtrapolationException,
ConnectivityException) as e:
# print(e)
pass
except (KeyboardInterrupt):
pass
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))
red_request.leds = orange_leds_array
else:
red_request.leds = red_leds_array
previous_req.leds = led_serv.call(red_request).previous_leds
def reset_leds():
global leds_red, previous_req
leds_red = False
led_serv.call(previous_req)
# wait a moment on startup, otherwise we will think there is a problem while ros control is still booting
set_leds_service = node.create_client(Leds, "/set_leds")
set_leds_service.wait_for_service()
node.get_clock().sleep_for(Duration(seconds=1))
node.create_subscription(DiagnosticStatus, "/diagnostics_toplevel_state", cb,
1)
rate = node.create_rate(100)
while rclpy.ok():
if last_hardware_error_time is not None:
current_time = node.get_clock().now().to_sec()
if currently_blinking:
# we are currently blinking, check if the last hardware error is to long ago
if current_time - last_hardware_error_time > ERROR_TIMEOUT:
currently_blinking = False
reset_leds()
led_set_time = current_time
else:
pos_msg.positions = [0, 0]
pos_msg.velocities = [1.5, 1.5]
pos_msg.accelerations = [-1, -1]
pos_msg.max_currents = [-1, -1]
rclpy.wait_for_service("bio_ik/get_bio_ik")
bio_ik = node.create_client(GetIK, 'bio_ik/get_bio_ik')
publish_motor_goals = node.create_publisher(JointCommand,
'head_motor_goals', 10)
while rclpy.ok():
x = float(input('x: '))
y = float(input('y: '))
point = PointStamped()
point.header.stamp = node.get_clock().now()
point.header.frame_id = base_footprint_frame
point.point.x = x
point.point.y = y
point.point.z = 0
###### Movement ######
point = tf_buffer.transform(point,
head_tf_frame,
timeout=Duration(seconds=0.3)).point
target = Point(point.x, point.y, point.z)
request.look_at_goals[0].target = target
response = bio_ik(request).ik_response
states = response.solution.joint_state
head_pan = states.position[states.name.index('HeadPan')]
head_tilt = states.position[states.name.index('HeadTilt')]
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_srvs.srv import Empty
from bitbots_msgs.msg import Buttons
from rclpy.node import Node
from rclpy.duration import Duration
rclpy.init(args=None)
node = Node('zero_on_button')
zero_l = node.create_client(Empty, "/foot_pressure_left/set_foot_zero")
zero_r = node.create_client(Empty, "/foot_pressure_right/set_foot_zero")
button_prev_state = False
press_time = node.get_clock().now() - Duration(seconds=1.0)
def cb(msg):
global button_prev_state, press_time
print("New msg")
print(msg.button1)
print(not button_prev_state)
print(node.get_clock().now() - press_time > Duration(seconds=1.0))
if msg.button3 and not button_prev_state and node.get_clock().now(
) - press_time > Duration(seconds=1.0):
zero_l()
zero_r()
press_time = node.get_clock()
button_prev_state = msg.button3
