diff --git a/Jenkinsfile b/Jenkinsfile deleted file mode 100644 index e9431714e..000000000 --- a/Jenkinsfile +++ /dev/null @@ -1,9 +0,0 @@ -@Library('bitbots_jenkins_library') import de.bitbots.jenkins.*; - -defineProperties() - -def pipeline = new BitbotsPipeline(this, env, currentBuild, scm) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_moveit_bindings"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_odometry"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_splines"))) -pipeline.execute() diff --git a/bitbots_animation_server/bitbots_animation_server/animation.py b/bitbots_animation_server/bitbots_animation_server/animation.py index 741a750b6..6ad858690 100644 --- a/bitbots_animation_server/bitbots_animation_server/animation.py +++ b/bitbots_animation_server/bitbots_animation_server/animation.py @@ -18,8 +18,8 @@ class Animation: """ def __init__(self, name, keyframes, default_interpolator=None): - self.name = name - self.keyframes = keyframes + self.name: str = name + self.keyframes: list(Keyframe) = keyframes self.default_interpolator = default_interpolator @@ -37,7 +37,7 @@ def parse(info): return anim -def as_dict(anim): +def as_dict(anim: Animation): """ Convert an animation to builtin python types to make it serializable to formats like ``json``. diff --git a/bitbots_animation_server/bitbots_animation_server/animation_node.py b/bitbots_animation_server/bitbots_animation_server/animation_node.py index 54c2ae821..69caa5d9f 100755 --- a/bitbots_animation_server/bitbots_animation_server/animation_node.py +++ b/bitbots_animation_server/bitbots_animation_server/animation_node.py @@ -1,131 +1,132 @@ #!/usr/bin/env python3 import json - -from rclpy.action import ActionClient import traceback import numpy as np import rclpy +from rclpy.action import ActionServer +from rclpy.action.server import ServerGoalHandle +from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.duration import Duration -from rclpy.node import Node +from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor -from rclpy.callback_groups import ReentrantCallbackGroup - -from humanoid_league_msgs.action import PlayAnimation -from humanoid_league_msgs.msg import Animation as AnimationMsg +from rclpy.node import Node +from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory +from bitbots_animation_server.animation import Animation from bitbots_animation_server.animation import parse -from sensor_msgs.msg import Imu, JointState -from bitbots_animation_server.resource_manager import find_all_animations_by_name -from humanoid_league_msgs.msg import RobotControlState +from bitbots_animation_server.resource_manager import ResourceManager from bitbots_animation_server.spline_animator import SplineAnimator -from rclpy.action import ActionServer -from rclpy.executors import ExternalShutdownException +from bitbots_msgs.action import PlayAnimation +from bitbots_msgs.msg import Animation as AnimationMsg, RobotControlState class AnimationNode(Node): - _feedback = PlayAnimation.Feedback - _result = PlayAnimation.Result - + """This node provides an action server for playing animations.""" + def __init__(self): """Starts a simple action server and waits for requests.""" - super().__init__("animation") - # currently, we set log level to info since the action server is spamming too much - if not self.get_parameter("use_sim_time"): - pass # todo how does this work in rclpy - # rospy.on_shutdown(self.on_shutdown_hook) + super().__init__("animation_server") self.get_logger().debug("Starting Animation Server") self.current_joint_states = None self.hcm_state = 0 self.current_animation = None - self.animation_cache = {} - all_animations = find_all_animations_by_name(self) + self.animation_cache: dict[str, Animation] = {} + + # Get robot type and create resource manager + self.declare_parameter("robot_type", "wolfgang") + self.resource_manager = ResourceManager(self.get_parameter('robot_type').value) + + # Load all animations into memory + all_animations = self.resource_manager.find_all_animations_by_name(self) for animation_name, animation_file in all_animations.items(): try: with open(animation_file) as fp: self.animation_cache[animation_name] = parse(json.load(fp)) except IOError: - self.get_logger().error("Animation '%s' could not be loaded" % animation_name) + self.get_logger().error(f"Animation '{animation_name}' could not be loaded") except ValueError: self.get_logger().error( - "Animation '%s' had a ValueError. Probably there is a syntax error in the animation file. " - "See traceback" % animation_name) + f"Animation '{animation_name}' had a ValueError. " + "Probably there is a syntax error in the animation file. " + "See traceback") traceback.print_exc() - # predefined messages for performance - self.anim_msg = AnimationMsg() - # AnimationMsg takes a JointTrajectory message to also be able to process trajectories. To keep this - # functionality, we use this message type, even though we only need a single joint goal in this case. - self.traj_msg = JointTrajectory() - self.traj_point = JointTrajectoryPoint() - callback_group = ReentrantCallbackGroup() self.create_subscription(JointState, "joint_states", self.update_current_pose, 1, callback_group=callback_group) self.create_subscription(RobotControlState, "robot_state", self.update_hcm_state, 1, callback_group=callback_group) + self.hcm_publisher = self.create_publisher(AnimationMsg, "animation", 1) self._as = ActionServer(self, PlayAnimation, "animation", self.execute_cb, callback_group=callback_group) - def on_shutdown_hook(self): - # we got external shutdown, let's still wait a bit, since we probably want to do a shutdown animation - self.get_clock().sleep_for(Duration(seconds=5)) - - def execute_cb(self, goal): + def execute_cb(self, goal: ServerGoalHandle): """ This is called, when someone calls the animation action""" + + # Set type for request + request: PlayAnimation.Goal = goal.request + first = True - self.current_animation = goal.request.animation + self.current_animation = request.animation # publish info to the console for the user - self.get_logger().info(f"Request to play animation {goal.request.animation}") + self.get_logger().info(f"Request to play animation {request.animation}") - if self.hcm_state != 0 and not goal.request.hcm: # 0 means controllable + if self.hcm_state != 0 and not request.hcm: # 0 means controllable # we cant play an animation right now # but we send a request, so that we may can soon self.send_animation_request() - self.get_logger().info("HCM not controllable. Only sent request to make it come controllable.") - goal.abort() - return PlayAnimation.Result(successful=False) - animator = self.get_animation_splines(self.current_animation, goal) + # Wait for the hcm to be controllable + num_tries = 0 + while rclpy.ok() and self.hcm_state != 0 and num_tries < 10: + num_tries += 1 + self.get_logger().info(f"HCM not controllable. Waiting... (try {num_tries})") + self.get_clock().sleep_until(self.get_clock().now() + Duration(seconds=0.1)) + + if self.hcm_state != 0: + self.get_logger().info( + "HCM not controllable. Only sent request to make it come controllable, " + "but it was not successful until timeout") + goal.abort() + return PlayAnimation.Result(successful=False) + + animator = self.get_animation_splines(self.current_animation) while rclpy.ok() and animator: try: last_time = self.get_clock().now() - # first check if we have another goal - # todo this does not work in ros2 - # self.check_for_new_goal(goal) - # new_goal = self._as.current_goal.goal.animation - ## if there is a new goal, calculate new splines and reset the time - # if new_goal != self.current_animation: - # self.current_animation = new_goal - # animator = self.get_animation_splines(self.current_animation) - # first = True # if we're here we want to play the next keyframe, cause there is no other goal # compute next pose - t = float(self.get_clock().now().seconds_nanoseconds()[0] + - self.get_clock().now().seconds_nanoseconds()[1] / 1e9) - animator.get_start_time() + t = self.get_clock().now().nanoseconds / 1e9 - animator.get_start_time() pose = animator.get_positions_rad(t) if pose is None: - # see walking node reset - # animation is finished # tell it to the hcm - self.send_animation(False, True, goal.request.hcm, None, None) + self.send_animation( + first=False, + last=True, + hcm=request.hcm, + pose=None, + torque=None) goal.publish_feedback(PlayAnimation.Feedback(percent_done=100)) # we give a positive result goal.succeed() return PlayAnimation.Result(successful=True) - self.send_animation(first, False, goal.request.hcm, pose, animator.get_torque(t)) + self.send_animation( + first=first, + last=False, + hcm=request.hcm, + pose=pose, + torque=animator.get_torque(t)) first = False # we have sent the first frame, all frames after this can't be the first - perc_done = int(((float(self.get_clock().now().seconds_nanoseconds()[0] + - self.get_clock().now().seconds_nanoseconds()[ - 1] / 1e9) - animator.get_start_time()) / animator.get_duration()) * 100) + perc_done = int(((self.get_clock().now().nanoseconds / 1e9 - animator.get_start_time()) / animator.get_duration()) * 100) perc_done = max(0, min(perc_done, 100)) goal.publish_feedback(PlayAnimation.Feedback(percent_done=perc_done)) @@ -135,65 +136,52 @@ def execute_cb(self, goal): exit() return PlayAnimation.Result(successful=False) - def get_animation_splines(self, animation_name, goal): + def get_animation_splines(self, animation_name: str): if animation_name not in self.animation_cache: - self.get_logger().error("Animation '%s' not found" % animation_name) + self.get_logger().error(f"Animation '{animation_name}' not found") return - - parsed_animation = self.animation_cache[animation_name] - return SplineAnimator(parsed_animation, self.current_joint_states, self.get_logger(), - self.get_clock()) - - def check_for_new_goal(self, goal): - if self._as.is_new_goal_available(): - next_goal = self._as.next_goal - if not next_goal or not next_goal.get_goal(): - return - self.get_logger().debug("New goal: " + next_goal.get_goal().animation) - if next_goal.get_goal().hcm: - self.get_logger().debug("Accepted hcm animation %s", next_goal.get_goal().animation) - # cancel old stuff and restart - self._as.current_goal.set_aborted() - self._as.accept_new_goal() - else: - # can't run this animation now - self._as.next_goal.set_rejected() - # delete the next goal to make sure, that we can accept something else - self._as.next_goal = None - self.get_logger().warn("Couldn't start non hcm animation because another one is already running.") + return SplineAnimator( + self.animation_cache[animation_name], + self.current_joint_states, + self.get_logger(), + self.get_clock()) def update_current_pose(self, msg): """Gets the current motor positions and updates the representing pose accordingly.""" self.current_joint_states = msg - def update_hcm_state(self, msg): + def update_hcm_state(self, msg: RobotControlState): self.hcm_state = msg.state def send_animation_request(self): - self.anim_msg.request = True - self.anim_msg.header.stamp = self.get_clock().now().to_msg() - self.hcm_publisher.publish(self.anim_msg) - - def send_animation(self, first, last, hcm, pose, torque): - self.anim_msg.request = False - self.anim_msg.first = first - self.anim_msg.last = last - self.anim_msg.hcm = hcm - if pose is not None: - self.traj_msg.joint_names = [] - self.traj_msg.points = [JointTrajectoryPoint()] + anim_msg = AnimationMsg() + anim_msg.request = True + anim_msg.header.stamp = self.get_clock().now().to_msg() + self.hcm_publisher.publish(anim_msg) + + def send_animation(self, first: bool, last: bool, hcm: bool, pose: dict, torque: dict): + """Sends an animation to the hcm""" + anim_msg = AnimationMsg() + anim_msg.request = False + anim_msg.first = first + anim_msg.last = last + anim_msg.hcm = hcm + if pose is not None:# + traj_msg = JointTrajectory() + traj_msg.joint_names = [] + traj_msg.points = [JointTrajectoryPoint()] # We are only using a single point in the trajectory message, since we only want to send a single joint goal - self.traj_msg.points[0].positions = [] - self.traj_msg.points[0].effort = [] + traj_msg.points[0].positions = [] + traj_msg.points[0].effort = [] for joint in pose: - self.traj_msg.joint_names.append(joint) - self.traj_msg.points[0].positions.append(pose[joint]) + traj_msg.joint_names.append(joint) + traj_msg.points[0].positions.append(pose[joint]) if torque: # 1 and 2 should be mapped to 1 - self.traj_msg.points[0].effort.append(np.clip((torque[joint]), 0, 1)) - self.anim_msg.position = self.traj_msg - self.anim_msg.header.stamp = self.get_clock().now().to_msg() - self.hcm_publisher.publish(self.anim_msg) + traj_msg.points[0].effort.append(np.clip((torque[joint]), 0, 1)) + anim_msg.position = traj_msg + anim_msg.header.stamp = self.get_clock().now().to_msg() + self.hcm_publisher.publish(anim_msg) def main(args=None): diff --git a/bitbots_animation_server/bitbots_animation_server/resource_manager.py b/bitbots_animation_server/bitbots_animation_server/resource_manager.py index c4e686c7f..4e37651a8 100644 --- a/bitbots_animation_server/bitbots_animation_server/resource_manager.py +++ b/bitbots_animation_server/bitbots_animation_server/resource_manager.py @@ -2,35 +2,26 @@ ResourceManager ^^^^^^^^^^^^^^^ -The ResourceManager module provides functions for file searching in a -Darwin Project. Thus, it is possible to find resources without knowing -the current location in the file system. - -This module provides the global methods :func:`find_resource`, -:func:`find_anim` and :func:`find` which use a single global instance -of the :class:`ResourceManager`. Thereby, files that have once been -discovered do not have to be searched again. +The ResourceManager module provides functions to search and list animations for a given robot. +Thus, it is possible to find resources without knowing the current location in the file system. """ -import os.path +import os from os.path import abspath, dirname, exists, join, normpath from os import walk -import rclpy from ament_index_python import get_package_share_directory -from rclpy.node import Node -class ResourceManager(object): +class ResourceManager: - 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") + def __init__(self, robot_type: str): + # Get the path to the animations folder + self.basepath = abspath(os.path.join( + get_package_share_directory(robot_type + "_animations"), + "animations")) self.cache = {} - self.files = [] # Animations cached for find_all_animations + self.files = [] self.names = [] # Plain animation names, without filename-extension self.animpath = self._get_animpath() @@ -117,19 +108,6 @@ def find(self, name, filename=""): return result - def generate_find(self, basepath): - """ Return a find function that automatically adds a basepath to - each name - :param basepath: The path to add to each file - :type basepath: String - - The returned function takes one argument which will be added to the - basepath before calling the normal "find" method without optional - arguments. - """ - path = normpath(basepath) - return lambda name: self.find(join(path, name)) - def find_animation(self, name): """ Find an animation in _animations/animations/*. The filename @@ -176,26 +154,3 @@ def find_all_animations_by_name(self, force_reload=False): if not self.files or force_reload: self.find_all_animations(force_reload) return dict(zip(self.names, self.files)) - - def find_all_animation_names(self, force_reload=False): - """ Same as find_all_animations, but returns a sorted set of the animations - for use in the record-ui - """ - if not self.names or force_reload: - self.find_all_animations(force_reload) - return sorted(set(self.names)) - - def is_animation_name(self, name, force_reload=False): - """Check if a name belongs to a saved animation - """ - if not self.names or force_reload: - self.find_all_animations(force_reload=True) - return name in self.names - -_RM = None # type: ResourceManager -# Shortcut to search for animations -def find_all_animations_by_name(node, *args, **kwargs): - global _RM - if not _RM: - _RM = ResourceManager(node) - return _RM.find_all_animations_by_name(*args, **kwargs) diff --git a/bitbots_animation_server/bitbots_animation_server/spline_animator.py b/bitbots_animation_server/bitbots_animation_server/spline_animator.py index 1ab5cb153..fada74daa 100644 --- a/bitbots_animation_server/bitbots_animation_server/spline_animator.py +++ b/bitbots_animation_server/bitbots_animation_server/spline_animator.py @@ -1,18 +1,21 @@ import math -import rclpy -from rclpy.node import Node +from rclpy.impl.rcutils_logger import RcutilsLogger as Logger +from rclpy.clock import Clock + +from sensor_msgs.msg import JointState +from bitbots_animation_server.animation import Animation from bitbots_splines.smooth_spline import SmoothSpline class SplineAnimator: - - def __init__(self, animation, current_joint_states, logger, clock): + def __init__(self, animation: Animation, current_joint_states: JointState, logger: Logger, clock: Clock): self.anim = animation - self.start_time = float(clock.now().seconds_nanoseconds()[0] + clock.now().seconds_nanoseconds()[1]/1e9) - self.animation_duration = 0 - self.current_point_time = 0 - self.spline_dict = {} + self.start_time = clock.now().nanoseconds /1e9 + + self.animation_duration = 0.0 + self.current_point_time = 0.0 + self.spline_dict: dict[str, SmoothSpline] = {} self.torques = {} # add current joint positions as start diff --git a/bitbots_animation_server/docs/manual/animation.rst b/bitbots_animation_server/docs/manual/animation.rst index b939b9b1e..9041231a8 100644 --- a/bitbots_animation_server/docs/manual/animation.rst +++ b/bitbots_animation_server/docs/manual/animation.rst @@ -1,7 +1,7 @@ Description of Animation Server =============================== -The Animation Server is responsible for playback of previously recorded keyframe animations. The server is called by the 'HCM', after the robot fell down and wants to get back up. It can also be used by the 'Behavior', e.g. for shooting. +The Animation Server is responsible for playback of previously recorded keyframe animations. The Animation Server is a ROS Action Server. For an action a message is sent to the Animation Server. This is similar to sending a normal 'message', the connection in this case however is not only one-way. The Action Server sends back the status of the active action as well as the result (successful or not). @@ -9,7 +9,7 @@ Actively running animations, can be interrupted by the HCM. This ensures that, s Animations consist of a series of keyframes. Each keyframe is a snapshot of motor positions at a certain point in time. During playback a file containing the recorded keyframes is read and each frame is played one after the other at a given interval. To achieve a fluid motion the frequency of interpolation is set to 200 Hz with the help of quintic splines. This interpolation is done in the 'Joint Space' (inbetween the motor positions, not inbetween the actual positions of the robot's extremities in the Cartesian space), due to the development of the project and better usability. -Animations can be run manually with `rosrun bitbots_animation_server run_animation `. +Animations can be run manually with `ros2 run bitbots_animation_server run_animation `. All animations can be found in the package `wolfgang_animations`. If an animation fails to run, the first thing to check is, if the HCM outputs a different 'Robot State' than 'Controllable' or 'Walking'. diff --git a/bitbots_animation_server/launch/test.launch b/bitbots_animation_server/launch/test.launch index 831b0e4e9..ed669e8f9 100644 --- a/bitbots_animation_server/launch/test.launch +++ b/bitbots_animation_server/launch/test.launch @@ -7,9 +7,9 @@ - - - - - + + + + + \ No newline at end of file diff --git a/bitbots_animation_server/package.xml b/bitbots_animation_server/package.xml index 07272c331..be494fc64 100644 --- a/bitbots_animation_server/package.xml +++ b/bitbots_animation_server/package.xml @@ -14,11 +14,13 @@ MIT + bitbots_docs + bitbots_msgs + bitbots_robot_description + bitbots_splines + bitbots_utils rclpy std_msgs - humanoid_league_msgs - bitbots_splines - bitbots_docs diff --git a/bitbots_animation_server/scripts/animation_hcm_bridge.py b/bitbots_animation_server/scripts/animation_hcm_bridge.py index 81a92370f..058c408c3 100755 --- a/bitbots_animation_server/scripts/animation_hcm_bridge.py +++ b/bitbots_animation_server/scripts/animation_hcm_bridge.py @@ -1,11 +1,15 @@ #!/usr/bin/env python3 + +""" +This script subscribes to the topic "animation" and publishes the received joint commands to the motor command topic, skipping the HCM. +""" + import rclpy from rclpy.node import Node -from rclpy.duration import Duration from bitbots_msgs.msg import JointCommand # List of all joint names. Do not change the order as it is important for Gazebo -from humanoid_league_msgs.msg import Animation +from bitbots_msgs.msg import Animation JOINT_NAMES = ['HeadPan', 'HeadTilt', 'LShoulderPitch', 'LShoulderRoll', 'LElbow', 'RShoulderPitch', 'RShoulderRoll', 'RElbow', 'LHipYaw', 'LHipRoll', 'LHipPitch', 'LKnee', 'LAnklePitch', @@ -26,8 +30,6 @@ def __init__(self): self.create_subscription(Animation, "animation", self.animation_cb, 10) - rclpy.spin(self) - def animation_cb(self, msg: Animation): self.joint_command_msg.header.stamp = self.get_clock().now().to_msg() for i in range(len(msg.position.joint_names)): @@ -39,4 +41,5 @@ def animation_cb(self, msg: Animation): if __name__ == '__main__': - bridge = AnimationHcmBridge() \ No newline at end of file + bridge = AnimationHcmBridge() + rclpy.spin(bridge) diff --git a/bitbots_animation_server/scripts/run_animation.py b/bitbots_animation_server/scripts/run_animation.py index 41fc6ca96..e45157f11 100755 --- a/bitbots_animation_server/scripts/run_animation.py +++ b/bitbots_animation_server/scripts/run_animation.py @@ -1,62 +1,56 @@ #!/usr/bin/env python3 -import argparse +import sys +import threading -from rclpy.action import ActionClient -from actionlib_msgs.msg import GoalStatus import rclpy -from rclpy.duration import Duration +from rclpy.action import ActionClient +from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node -import sys -import humanoid_league_msgs.action +from bitbots_msgs.action import PlayAnimation def anim_run(anim=None, hcm=False): node = Node("run_animation") - anim_client = ActionClient(node, humanoid_league_msgs.action.PlayAnimation, 'animation') + + # Create own executor for Python part + multi_executor = MultiThreadedExecutor(num_threads=4) + multi_executor.add_node(node) + spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True) + spin_thread.start() + + anim_client = ActionClient(node, PlayAnimation, 'animation') + if anim is None or anim == "": node.get_logger().warn("Tried to play an animation with an empty name!") return False + first_try = anim_client.wait_for_server(3.0) + if not first_try: node.get_logger().error( "Animation Action Server not running! Motion can not work without animation action server. " "Will now wait until server is accessible!") anim_client.wait_for_server() node.get_logger().warn("Animation server now running, hcm will go on.") - goal = humanoid_league_msgs.action.PlayAnimation.Goal() + + goal = PlayAnimation.Goal() goal.animation = anim goal.hcm = hcm - # todo not .send_goal does never return - state = anim_client.send_goal_async(goal) - if state == GoalStatus.PENDING: - print('Pending') - elif state == GoalStatus.ACTIVE: - print('Active') - elif state == GoalStatus.PREEMPTED: - print('Preempted') - elif state == GoalStatus.SUCCEEDED: - print('Succeeded') - elif state == GoalStatus.ABORTED: - print('Aborted') - elif state == GoalStatus.REJECTED: - print('Rejected') - elif state == GoalStatus.PREEMPTING: - print('Preempting') - elif state == GoalStatus.RECALLING: - print('Recalling') - elif state == GoalStatus.RECALLED: - print('Recalled') - elif state == GoalStatus.LOST: - print('Lost') - else: - print('Unknown state', state) + + print(f"Sending animation {anim} to {'hcm' if hcm else 'motion'}.") + state: PlayAnimation.Result = anim_client.send_goal(goal).result + + print(f"Animation {anim} {['failed', 'successfully finished'][int(state.successful)]}.") + + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': rclpy.init(args=sys.argv) - # run with "rosrun bitbots_animation_server run_animation.py NAME" + # run with "ros2 run bitbots_animation_server run_animation.py NAME" if len(sys.argv) > 1: # Support for _anim:=NAME -style execution for legacy reasons if sys.argv[1].startswith('_anim:=') or sys.argv[1].startswith('anim:='): diff --git a/bitbots_dynamic_kick/CMakeLists.txt b/bitbots_dynamic_kick/CMakeLists.txt index f232b08e6..405825b00 100644 --- a/bitbots_dynamic_kick/CMakeLists.txt +++ b/bitbots_dynamic_kick/CMakeLists.txt @@ -16,9 +16,7 @@ find_package(control_msgs REQUIRED) find_package(control_toolbox REQUIRED) find_package(Eigen3 REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(humanoid_league_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) -find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(rot_conv REQUIRED) @@ -61,9 +59,7 @@ ament_target_dependencies(KickNode control_msgs control_toolbox geometry_msgs - humanoid_league_msgs moveit_ros_planning_interface - nav_msgs rclcpp rot_conv sensor_msgs @@ -92,9 +88,7 @@ target_link_libraries(KickNode "${cpp_typesupport_target}") # control_msgs # control_toolbox # geometry_msgs -# humanoid_league_msgs # moveit_ros_planning_interface -# nav_msgs # rclcpp # ros2_python_extension # rot_conv diff --git a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg b/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg deleted file mode 100755 index eaab2e9bc..000000000 --- a/bitbots_dynamic_kick/cfg/bitbots_dynamic_kick_params.cfg +++ /dev/null @@ -1,88 +0,0 @@ -#!/usr/bin/env python3 - -PACKAGE = 'bitbots_dynamic_kick' -import roslib - -roslib.load_manifest(PACKAGE) - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -group_engine = gen.add_group("Engine", type="tab") -group_engine_distances = group_engine.add_group("Distances and Positions") -group_engine.add("engine_rate", int_t, 1, - "How often the engine updates motor goals [Hz]", - min=1, max=500) -group_engine_distances.add("foot_rise", double_t, 2, - "Rise of the foot [m]", - min=0, max=2) -group_engine_distances.add("foot_distance", double_t, 2, - "How far apart the feet should be from each other. This is only relevant during the initial phase " - "when the foot is not yet moving towards the ball [m]", - min=0, max=3) -group_engine_distances.add("kick_windup_distance", double_t, 2, - "How far away from the ball to stop and perform the actual (fast) kick movement [m]", - min=0, max=2) -group_engine_distances.add("trunk_height", double_t, 4, - "Height of the trunk [m]", - min=0, max=0.6) -group_engine_distances.add("trunk_roll", double_t, 4, - "Roll of the trunk, positive means toward the support foot [rad]", - min=-1, max=1) -group_engine_distances.add("trunk_pitch", double_t, 4, - "Pitch of the trunk, positive means toward the front [rad]", - min=-1, max=1) -group_engine_distances.add("trunk_yaw", double_t, 4, - "Yaw of the trunk, positive means turning toward the kicking foot [rad]", - min=-1, max=1) - -group_engine_timings = group_engine.add_group("Timings") -group_engine_timings.add("move_trunk_time", double_t, 3, - "How long it should take to move the trunk above the support foot before raising the flying foot [s]", - min=0) -group_engine_timings.add("raise_foot_time", double_t, 3, - "How long it should take to raise the flying foot from the ground [s]", - min=0) -group_engine_timings.add("move_to_ball_time", double_t, 3, - "How long it should take to move the raised foot to the windup point [s]", - min=0) -group_engine_timings.add("kick_time", double_t, 3, # TODO replace this with dynamic calculation - "This will be removed in the future [s]", - min=0) -group_engine_timings.add("move_back_time", double_t, 3, - "How long it should take to move the flying foot back after kicking [s]", - min=0) -group_engine_timings.add("lower_foot_time", double_t, 3, - "How long it should take to lower the foot [s]", - min=0) -group_engine_timings.add("move_trunk_back_time", double_t, 3, - "How long it should take to move the flying foot back after kicking [s]", - min=0) - -group_engine_decisions = group_engine.add_group("Decisions") -group_engine_decisions.add("choose_foot_corridor_width", double_t, 3, - "How wide the corridor should be in which advanced foot-choosing calculations take place." - "If kick goal is placed outside this corridor, the foot on that side will automatically be assigned [m]" - "the flying foot.", - min=0) - -group_stabilizing = gen.add_group("Stabilizer", type="tab") - -group_stabilizing_cop = group_stabilizing.add_group("CenterOfPressure regulation") -group_stabilizing_cop.add("use_center_of_pressure", bool_t, 4, - "Use the center of pressure for stabilizing") -group_stabilizing_cop.add("stabilizing_point_x", double_t, 4, - "Where to stabilize over in x direction. Measured from the support foots *_sole frame [m]", - min=-0.5, max=0.5) -group_stabilizing_cop.add("stabilizing_point_y", double_t, 4, - "Where to stabilize over in y direction. Measured from the support foots *_sole frame. " - "Be aware that - always represents the direction towards base_footprint [m]", - min=-0.5, max=0.5) - -group_visualization = gen.add_group("Visualization", type="tab") -group_visualization.add("spline_smoothness", int_t, 8, - "how many points to extract from splines for visualization", - min=1, max=200) - -exit(gen.generate(PACKAGE, 'dynamic_kick', 'DynamicKick')) diff --git a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h index bd56fc123..99243e596 100644 --- a/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h +++ b/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h @@ -101,6 +101,7 @@ class KickNode : public rclcpp::Node{ moveit::core::RobotStatePtr goal_state_; moveit::core::RobotStatePtr current_state_; OnSetParametersCallbackHandle::SharedPtr callback_handle_; + bool currently_kicking_ = false; std::string base_link_frame_, base_footprint_frame_, l_sole_frame_, r_sole_frame_; @@ -133,11 +134,6 @@ class KickNode : public rclcpp::Node{ */ void publishSupportFoot(bool is_left_kick); - /** - * Helper method to achieve correctly sampled rate - */ - double getTimeDelta(); - /** * Get JointCommand message for JointGoals */ diff --git a/bitbots_dynamic_kick/launch/test.launch b/bitbots_dynamic_kick/launch/test.launch index 8bb2a48c4..2480bca11 100644 --- a/bitbots_dynamic_kick/launch/test.launch +++ b/bitbots_dynamic_kick/launch/test.launch @@ -6,14 +6,30 @@ - + + + + + + + + + + + + + + + + + - --> + @@ -21,14 +37,14 @@ - --> + - --> + diff --git a/bitbots_dynamic_kick/launch/viz.launch b/bitbots_dynamic_kick/launch/viz.launch index 4bdc594e9..0aa37e17e 100644 --- a/bitbots_dynamic_kick/launch/viz.launch +++ b/bitbots_dynamic_kick/launch/viz.launch @@ -1,8 +1,21 @@ - + + + + + + + + + + + + + + diff --git a/bitbots_dynamic_kick/package.xml b/bitbots_dynamic_kick/package.xml index ca56651bb..0c3c650db 100644 --- a/bitbots_dynamic_kick/package.xml +++ b/bitbots_dynamic_kick/package.xml @@ -22,25 +22,28 @@ rosidl_default_generators bio_ik + backward_ros + biped_interfaces bitbots_docs bitbots_msgs + bitbots_robot_description bitbots_splines bitbots_utils control_toolbox geometry_msgs - humanoid_league_msgs moveit_core moveit_ros_move_group - moveit_ros_robot_interaction moveit_ros_planning_interface + moveit_ros_robot_interaction rclcpp ros2_python_extension rot_conv + sensor_msgs std_msgs - tf2 tf2_eigen tf2_geometry_msgs tf2_ros + tf2 rosidl_interface_packages diff --git a/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_dynamic_kick/src/kick_node.cpp index 807b43e28..7694f7cca 100644 --- a/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_dynamic_kick/src/kick_node.cpp @@ -260,6 +260,8 @@ rclcpp_action::CancelResponse KickNode::cancelCb(std::shared_ptr goal) { + // Set flag so no other goal can be accepted + currently_kicking_ = true; // this needs to return quickly to avoid blocking the executor, so spin up a new thread std::thread{std::bind(&KickNode::executeCb, this, std::placeholders::_1), goal}.detach(); } @@ -269,6 +271,10 @@ rclcpp_action::GoalResponse KickNode::goalCb( std::shared_ptr goal) { RCLCPP_INFO(this->get_logger(), "Received goal request"); (void) uuid; + if (currently_kicking_) { + RCLCPP_INFO(this->get_logger(), "Already kicking, rejecting new goal"); + return rclcpp_action::GoalResponse::REJECT; + } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -312,56 +318,54 @@ void KickNode::executeCb(const std::shared_ptr goal_handle) { result->result = bitbots_msgs::action::Kick::Result::SUCCESS; goal_handle->succeed(result); } + // Reset flag so new goals can be accepted + currently_kicking_ = false; } -double KickNode::getTimeDelta() { - // compute actual time delta that happened - double dt; - double current_ros_time = this->get_clock()->now().seconds(); +void KickNode::loopEngine(const std::shared_ptr> goal_handle) { + // Store the time of the last execution + auto last_time = this->get_clock()->now(); + auto next_execution_time {last_time}; - // first call needs to be handled specially - if (last_ros_update_time_ == 0) { - last_ros_update_time_ = current_ros_time; - return 0.001; - } + // Loop to perform the kick trajectory + while (rclcpp::ok()) { + // Store time of this step + auto step_time = this->get_clock()->now(); - dt = current_ros_time - last_ros_update_time_; - // this can happen due to floating point precision or in simulation - if (dt == 0) { - RCLCPP_WARN(this->get_logger(), "dynamic kick: dt was 0"); - dt = 0.001; - } - last_ros_update_time_ = current_ros_time; + // Check if enough time has passed to execute the next step + if (step_time >= next_execution_time) { + + // Determine the next execution time + next_execution_time = step_time + rclcpp::Duration::from_seconds(1.0 / engine_rate_); - return dt; -} + // Get the time since the last execution + auto dt = step_time - last_time; -void KickNode::loopEngine(const std::shared_ptr> goal_handle) { - /* Do the loop as long as nothing cancels it */ - double dt; - rclcpp::Time next_loop_time; - rclcpp::Time last_time = this->get_clock()->now(); - while (rclcpp::ok()) { - next_loop_time = last_time + rclcpp::Duration::from_seconds(1.0 / engine_rate_); - last_time = this->get_clock()->now(); - if (this->get_clock()->sleep_until(next_loop_time)) { - dt = getTimeDelta(); - std::optional motor_goals = kickStep(dt); + // Calculate the kick joint goals + std::optional motor_goals = kickStep(dt.seconds()); - /* Publish feedback to the client */ + // Publish feedback to the client auto feedback = std::make_shared(); feedback->percent_done = engine_.getPercentDone(); feedback->chosen_foot = engine_.isLeftKick() ? bitbots_msgs::action::Kick::Feedback::FOOT_LEFT - : bitbots_msgs::action::Kick::Feedback::FOOT_RIGHT; + : bitbots_msgs::action::Kick::Feedback::FOOT_RIGHT; goal_handle->publish_feedback(feedback); + // Send the joint goals to the robot + joint_goal_publisher_->publish(getJointCommand(motor_goals.value())); + + // Break if the kick is finished if (feedback->percent_done >= 100) { break; } - joint_goal_publisher_->publish(getJointCommand(motor_goals.value())); + + // Store the time of this execution + last_time = step_time; + } else { - usleep(1); + // Sleep one millisecond to avoid busy waiting + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } } diff --git a/bitbots_dynamic_kick/test/rostests/test_kick_runs.launch b/bitbots_dynamic_kick/test/rostests/test_kick_runs.launch deleted file mode 100644 index 88f597557..000000000 --- a/bitbots_dynamic_kick/test/rostests/test_kick_runs.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/bitbots_dynamic_kick/test/rostests/test_kick_runs.py b/bitbots_dynamic_kick/test/rostests/test_kick_runs.py deleted file mode 100755 index 3b04c3a7f..000000000 --- a/bitbots_dynamic_kick/test/rostests/test_kick_runs.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python3 -from bitbots_test.test_case import RosNodeTestCase -from bitbots_test.mocks import MockSubscriber -from actionlib_msgs.msg import GoalStatus -from bitbots_msgs.msg import KickAction, KickGoal, JointCommand -from geometry_msgs.msg import Quaternion -from rclpy.action import ActionClient -import rclpy -from rclpy.node import Node - - -class KickRunsTestCase(RosNodeTestCase): - def single_run(self, goal): - def done_cb(state, result): - assert state == GoalStatus.SUCCEEDED, "Kick was not successful" - self.kick_succeeded = True - - self.kick_succeeded = False - - sub = MockSubscriber('kick_motor_goals', JointCommand) - self.with_assertion_grace_period(lambda: self.assertNodeRunning("dynamic_kick"), 20) - client = ActionClient(self, KickAction, 'dynamic_kick') - assert client.wait_for_server(), "Kick action server not running" - - client.send_goal_async(goal) - client.done_cb = done_cb - client.wait_for_result() - sub.wait_until_connected() - sub.assertMessageReceived() - assert self.kick_succeeded, "Kick was not successful" - - def test_normal_kick(self): - goal = KickGoal() - goal.header.stamp = self.get_clock().now() - goal.header.frame_id = "base_footprint" - goal.ball_position.x = 0.2 - goal.kick_direction = Quaternion(0, 0, 0, 1) - goal.kick_speed = 1.0 - self.single_run(goal) - - def test_unstable_kick(self): - goal = KickGoal() - goal.header.stamp = self.get_clock().now() - goal.header.frame_id = "base_footprint" - goal.ball_position.x = 0.2 - goal.kick_direction = Quaternion(0, 0, 0, 1) - goal.kick_speed = 1.0 - goal.unstable = True - self.single_run(goal) - -if __name__ == '__main__': - from bitbots_test import run_rostests - run_rostests(KickRunsTestCase) diff --git a/bitbots_dynup/bitbots_dynup_py/py_dynup_wrapper.py b/bitbots_dynup/bitbots_dynup_py/py_dynup_wrapper.py index 915f388a4..5a78cdb5b 100644 --- a/bitbots_dynup/bitbots_dynup_py/py_dynup_wrapper.py +++ b/bitbots_dynup/bitbots_dynup_py/py_dynup_wrapper.py @@ -1,16 +1,10 @@ from io import BytesIO -import rclpy -from rclpy.node import Node -from std_msgs.msg import Int64 - -from bitbots_dynup.py_dynup import PyDynupWrapper, spin_once from bitbots_dynup.msg import DynupPoses -from bitbots_msgs.msg import JointCommand, FootPressure -from geometry_msgs.msg import Twist, Pose, PoseArray -from sensor_msgs.msg import Imu, JointState -from std_msgs.msg import String -from nav_msgs.msg import Odometry +from bitbots_dynup.py_dynup import PyDynupWrapper, spin_once +from geometry_msgs.msg import PoseArray + +from bitbots_msgs.msg import JointCommand class PyDynup(object): diff --git a/bitbots_dynup/docs/index.rst b/bitbots_dynup/docs/index.rst index 3fc6db302..ad9f1579b 100644 --- a/bitbots_dynup/docs/index.rst +++ b/bitbots_dynup/docs/index.rst @@ -30,9 +30,9 @@ Since the arms are lacking degrees of freedom, the IK solver is set to also cons How to test it -------------- -To test the dynup system, simply start the test script with :code:`roslaunch bitbots_dynup test.launch sim:=true`. +To test the dynup system, simply start the test script with :code:`ros2 launch bitbots_dynup test.launch sim:=true`. The :code:`sim` parameter is only required, if the system is tested in simulation, otherwise omit it. -To execute the motion, run the following command: :code:`rosrun bitbots_dynup dummy_client.py `, replacing :code:`` with one of the six directions mentioned above. +To execute the motion, run the following command: :code:`ros2 run bitbots_dynup dummy_client.py `, replacing :code:`` with one of the six directions mentioned above. diff --git a/bitbots_dynup/include/bitbots_dynup/dynup_engine.h b/bitbots_dynup/include/bitbots_dynup/dynup_engine.h index ada2e3cc8..dd749733d 100644 --- a/bitbots_dynup/include/bitbots_dynup/dynup_engine.h +++ b/bitbots_dynup/include/bitbots_dynup/dynup_engine.h @@ -1,19 +1,19 @@ #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_ -#include -#include -#include -#include +#include "dynup_stabilizer.h" +#include +#include +#include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include #include -#include "dynup_stabilizer.h" namespace bitbots_dynup { @@ -25,10 +25,6 @@ class DynupEngine : public bitbots_splines::AbstractEngine - diff --git a/bitbots_dynup/launch/test.launch b/bitbots_dynup/launch/test.launch index d3cf6bc85..3ac4513f9 100644 --- a/bitbots_dynup/launch/test.launch +++ b/bitbots_dynup/launch/test.launch @@ -8,12 +8,14 @@ - - - - - - + + + + + + + + diff --git a/bitbots_dynup/package.xml b/bitbots_dynup/package.xml index d5881f52a..52644dff9 100644 --- a/bitbots_dynup/package.xml +++ b/bitbots_dynup/package.xml @@ -13,30 +13,31 @@ Marc Bestmann ament_cmake - std_msgs - tf2 - tf2_ros - tf2_eigen - tf2_geometry_msgs + + backward_ros bio_ik - moveit_core - moveit_ros_move_group + bitbots_docs + bitbots_msgs + bitbots_robot_description bitbots_splines + bitbots_utils + control_toolbox geometry_msgs - bitbots_msgs - moveit_ros_robot_interaction + moveit_core + moveit_ros_move_group moveit_ros_planning_interface - bitbots_docs - rot_conv - control_toolbox + moveit_ros_robot_interaction ros2_python_extension - backward_ros + rot_conv + std_msgs + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2 plotjuggler bitbots_odometry - ros2launch - tested_integration diff --git a/bitbots_dynup/scripts/generate_walkready_animation.py b/bitbots_dynup/scripts/generate_walkready_animation.py deleted file mode 100644 index bcaa39729..000000000 --- a/bitbots_dynup/scripts/generate_walkready_animation.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python3 -import sys - -from rclpy.action import ActionClient -import rclpy -from rclpy.node import Node -from actionlib_msgs.msg import GoalStatus -from geometry_msgs.msg import Twist -from bitbots_msgs.msg import JointCommand, DynUpAction, DynUpGoal -import math - - -def callback(msg): - output_string = "{\n \"author\": \"walkready_script\", \n \"description\": \"none\",\n \"hostname\": \"tams159\", \n \"keyframes\": [ \n { \n \"duration\": 1.0, \n \"goals\": { \n" - i = 0 - for joint_name in msg.joint_names: - output_string += " \"" + str(joint_name) + "\" : " + str(math.degrees(msg.positions[i])) - if i < len(msg.joint_names) - 1: - output_string += "," - output_string += "\n" - i += 1 - output_string += "},\n \"name\": \"generated frame\", \n \"pause\": 0.5 \n }\n ], \n \"name\": \"walkready\", \n\"version\": 0 \n} \n" - with open("walkready.json", "w") as text_file: - text_file.write(output_string) - - -def done_cb(state, result): - print('Action completed: ', end='') - if state == GoalStatus.PENDING: - print('Pending') - elif state == GoalStatus.ACTIVE: - print('Active') - elif state == GoalStatus.PREEMPTED: - print('Preempted') - elif state == GoalStatus.SUCCEEDED: - print('Succeeded') - elif state == GoalStatus.ABORTED: - print('Aborted') - elif state == GoalStatus.REJECTED: - print('Rejected') - elif state == GoalStatus.PREEMPTING: - print('Preempting') - elif state == GoalStatus.RECALLING: - print('Recalling') - elif state == GoalStatus.RECALLED: - print('Recalled') - elif state == GoalStatus.LOST: - print('Lost') - else: - print('Unknown state', state) - print(str(result)) - - -rclpy.init(args=None) -self.get_logger().warn("Make sure that the Dynup is running together with simulator or this script will not work.") - -rospy.Subscriber("/DynamixelController/command", JointCommand, callback) - -print('[..] Connecting to action server \'dynup\'', end='') -sys.stdout.flush() -client = ActionClient(self, DynUpAction, 'dynup') -if not client.wait_for_server(): - exit(1) -print('\r[OK] Connecting to action server \'dynup\'') -print() - -goal = DynUpGoal() -goal.direction = "rise" - -client.send_goal_async(goal) -client.done_cb = done_cb -print("Sent new goal. Waiting for result") -client.wait_for_result() - -self.get_logger().info("Your walkready animation has been saved to the current directory.") diff --git a/bitbots_dynup/scripts/test_time.py b/bitbots_dynup/scripts/test_time.py deleted file mode 100644 index 391448762..000000000 --- a/bitbots_dynup/scripts/test_time.py +++ /dev/null @@ -1,165 +0,0 @@ -#!/usr/bin/env python3 - -from __future__ import print_function -import sys - -import rclpy -from rclpy.node import Node -from rclpy.action import ActionClient - -from actionlib_msgs.msg import GoalStatus -from bitbots_msgs.msg import DynUpGoal, DynUpAction, DynUpFeedback -import humanoid_league_msgs.msg -from sensor_msgs.msg import Imu - -showing_feedback = False - -if __name__ == "__main__": - def done_cb(state, result): - print('Action completed: ', end='') - if state == GoalStatus.PENDING: - print('Pending') - elif state == GoalStatus.ACTIVE: - print('Active') - elif state == GoalStatus.PREEMPTED: - print('Preempted') - elif state == GoalStatus.SUCCEEDED: - print('Succeeded') - elif state == GoalStatus.ABORTED: - print('Aborted') - elif state == GoalStatus.REJECTED: - print('Rejected') - elif state == GoalStatus.PREEMPTING: - print('Preempting') - elif state == GoalStatus.RECALLING: - print('Recalling') - elif state == GoalStatus.RECALLED: - print('Recalled') - elif state == GoalStatus.LOST: - print('Lost') - else: - print('Unknown state', state) - print(str(result)) - - - def active_cb(): - print("Server accepted action") - - - def feedback_cb(feedback): - if len(sys.argv) > 1 and '--feedback' in sys.argv: - print('Feedback') - print(feedback) - print() - - - def play_animation(anim): - first_try = anim_client.wait_for_server( - Duration(seconds=self.get_parameter('"hcm/anim_server_wait_time"').get_parameter_value().double_value - if not first_try: - self.get_logger().error( - "Animation Action Server not running! Motion can not work without animation action server. " - "Will now wait until server is accessible!") - anim_client.wait_for_server() - self.get_logger().warn("Animation server now running, hcm will go on.") - goal = humanoid_league_msgs.msg.PlayAnimationGoal() - goal.animation = anim - goal.hcm = False - state = anim_client.send_goal_and_wait(goal) - if state == GoalStatus.PENDING: - print('Pending') - elif state == GoalStatus.ACTIVE: - print('Active') - elif state == GoalStatus.PREEMPTED: - print('Preempted') - elif state == GoalStatus.SUCCEEDED: - print('Succeeded') - elif state == GoalStatus.ABORTED: - print('Aborted') - elif state == GoalStatus.REJECTED: - print('Rejected') - elif state == GoalStatus.PREEMPTING: - print('Preempting') - elif state == GoalStatus.RECALLING: - print('Recalling') - elif state == GoalStatus.RECALLED: - print('Recalled') - elif state == GoalStatus.LOST: - print('Lost') - else: - print('Unknown state', state) - - - def play_dynup(direction): - global last_move_time - goal = DynUpGoal() - goal.direction = direction - - client.done_cb = done_cb - client.feedback_cb = feedback_cb - client.active_cb = active_cb - client.send_goal_async(goal) - print("Sent new goal. Waiting for result") - client.wait_for_result() - - - last_move_time = None - - - def imu_cb(msg: Imu): - global last_move_time - if msg.angular_velocity.x > 0.15 or msg.angular_velocity.y > 0.15: - last_move_time = self.get_clock().now().to_sec() - - - rclpy.init(args=None) - print('[..] Connecting to action server \'dynup\'', end='') - sys.stdout.flush() - client = ActionClient(self, DynUpAction, 'dynup') - if not client.wait_for_server(): - exit(1) - print('\r[OK] Connecting to action server \'dynup\'') - - anim_client = ActionClient(self, humanoid_league_msgs.msg.PlayAnimationAction, 'animation') - - imu_sub = self.create_subscription(Imu, "/imu/data", imu_cb, 1) - - while rclpy.ok(): - direction = None - anim = None - inp = input("Which direction [f|b]") - if inp == "f": - direction = "front" - anim = "falling_front" - key_anim = "stand_up_front" - elif inp == "b": - direction = "back" - anim = "falling_back" - key_anim = "stand_up_back" - else: - print(f"input {inp} not valid") - continue - - input("Will now go into falling position. please hold robot and press enter") - play_animation(anim) - - inp = input("Please put robot on the ground. Then choose dynup or keyframe [d|k]") - if inp == "d": - last_move_time = None - start_time = self.get_clock().now().to_sec() - play_dynup(direction) - elif inp == "k": - last_move_time = None - start_time = self.get_clock().now().to_sec() - play_animation(key_anim) - else: - print("invalid input") - continue - - # wait till robot is standing at least 3 seconds not moving - while last_move_time is None or self.get_clock().now().to_sec() - last_move_time < 3: - self.get_clock().sleep_for(Duration(seconds=0.0001) - # compute duration - duration = last_move_time - start_time - - print(f"Dynup took {duration} s") diff --git a/bitbots_dynup/src/dynup_engine.cpp b/bitbots_dynup/src/dynup_engine.cpp index bd8eac13a..3f20406ca 100644 --- a/bitbots_dynup/src/dynup_engine.cpp +++ b/bitbots_dynup/src/dynup_engine.cpp @@ -677,20 +677,20 @@ void DynupEngine::setGoals(const DynupRequest &goals) { // get parameters from walking. If walking is not running, use default values // we re-request the values every time because they can be changed by dynamic reconfigure // and re-requesting them is fast enough - double foot_distance, trunk_x_final, trunk_pitch, trunk_height; - bool walking_running = false; std::vector walking_params; - if (walking_param_client_->service_is_ready()) { - walking_params = walking_param_client_->get_parameters({"engine.trunk_pitch", - "engine.trunk_height", - "engine.foot_distance", - "engine.trunk_x_offset"}, - std::chrono::duration(10)); - // when the walking was killed, service_is_ready is still true but the parameters come back empty - walking_running = !walking_params.empty(); - } - - if (walking_running) { + // Get params and wait for walking to be ready + walking_params = walking_param_client_->get_parameters({"engine.trunk_pitch", + "engine.trunk_height", + "engine.foot_distance", + "engine.trunk_x_offset"}, + std::chrono::seconds(10)); + + // when the walking was killed, service_is_ready is still true but the parameters come back empty + if (walking_params.size() != 4) { + RCLCPP_WARN(node_->get_logger(), "Walking is not running, using default parameters for walkready."); + } else { + // Placeholders for the parameters, we assure that we have all parameters before, so we can use -1 as a placeholder + double foot_distance = -1, trunk_x_final = -1, trunk_pitch = -1, trunk_height = -1; for (auto ¶m: walking_params) { if (param.get_name() == "engine.trunk_pitch") { trunk_pitch = param.get_value(); @@ -702,6 +702,7 @@ void DynupEngine::setGoals(const DynupRequest &goals) { trunk_x_final = param.get_value(); } } + // walking uses a different coordinate system for the trunk trunk_height = trunk_height * std::cos(trunk_pitch); trunk_x_final = trunk_x_final - std::sin(trunk_pitch) * trunk_height; @@ -709,8 +710,6 @@ void DynupEngine::setGoals(const DynupRequest &goals) { params_["trunk_height"] = rclcpp::Parameter("trunk_height", trunk_height); params_["foot_distance"] = rclcpp::Parameter("foot_distance", foot_distance); params_["trunk_x_final"] = rclcpp::Parameter("trunk_x_final", trunk_x_final); - } else { - RCLCPP_WARN(node_->get_logger(), "Walking is not running, using default parameters for walkready."); } if (goals.direction == "front") { diff --git a/bitbots_dynup/src/dynup_node.cpp b/bitbots_dynup/src/dynup_node.cpp index d8472dec3..e14fab146 100644 --- a/bitbots_dynup/src/dynup_node.cpp +++ b/bitbots_dynup/src/dynup_node.cpp @@ -340,11 +340,14 @@ bitbots_dynup::msg::DynupPoses DynupNode::getCurrentPoses() { /* Transform the left foot into the right foot frame and all other splines into the base link frame*/ bitbots_dynup::msg::DynupPoses msg; try { - //0.2 second timeout for transformations - geometry_msgs::msg::Transform l_foot_transformed = tf_buffer_->lookupTransform(r_sole_frame_, l_sole_frame_, time, tf2::durationFromSec(0.2)).transform; - geometry_msgs::msg::Transform r_foot_transformed = tf_buffer_->lookupTransform(base_link_frame_, r_sole_frame_, time, tf2::durationFromSec(0.2)).transform; - geometry_msgs::msg::Transform l_hand_transformed = tf_buffer_->lookupTransform(base_link_frame_, l_wrist_frame_, time, tf2::durationFromSec(0.2)).transform; - geometry_msgs::msg::Transform r_hand_transformed = tf_buffer_->lookupTransform(base_link_frame_, r_wrist_frame_, time, tf2::durationFromSec(0.2)).transform; + // Timeout for transformations + auto timeout = tf2::durationFromSec(1.0); + + // Get the transforms of the end effectors + geometry_msgs::msg::Transform l_foot_transformed = tf_buffer_->lookupTransform(r_sole_frame_, l_sole_frame_, time, timeout).transform; + geometry_msgs::msg::Transform r_foot_transformed = tf_buffer_->lookupTransform(base_link_frame_, r_sole_frame_, time, timeout).transform; + geometry_msgs::msg::Transform l_hand_transformed = tf_buffer_->lookupTransform(base_link_frame_, l_wrist_frame_, time, timeout).transform; + geometry_msgs::msg::Transform r_hand_transformed = tf_buffer_->lookupTransform(base_link_frame_, r_wrist_frame_, time, timeout).transform; std::function transform2pose = [](geometry_msgs::msg::Transform transform) { geometry_msgs::msg::Pose pose; diff --git a/bitbots_dynup/test/rostests/test_dynup_runs.launch b/bitbots_dynup/test/rostests/test_dynup_runs.launch deleted file mode 100644 index 2ae5ab423..000000000 --- a/bitbots_dynup/test/rostests/test_dynup_runs.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/bitbots_dynup/test/rostests/test_dynup_runs.py b/bitbots_dynup/test/rostests/test_dynup_runs.py deleted file mode 100755 index 29b4ede94..000000000 --- a/bitbots_dynup/test/rostests/test_dynup_runs.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python3 -from bitbots_test.test_case import RosNodeTestCase -from bitbots_test.mocks import MockSubscriber -from actionlib_msgs.msg import GoalStatus -from bitbots_msgs.msg import DynUpAction, DynUpGoal, JointCommand -from rclpy.action import ActionClient - - -class DynupRunsTestCase(RosNodeTestCase): - def one_run(self, direction): - def done_cb(state, result): - assert state == GoalStatus.SUCCEEDED, "Dynup was not successful" - self.kick_succeeded = True - - self.kick_succeeded = False - - sub = MockSubscriber('dynup_motor_goals', JointCommand) - self.with_assertion_grace_period(lambda: self.assertNodeRunning("dynup"), 20) - client = ActionClient(self, DynUpAction, 'dynup') - assert client.wait_for_server(), "Dynup action server not running" - - goal = DynUpGoal() - goal.direction = direction - client.send_goal_async(goal) - client.done_cb = done_cb - client.wait_for_result() - sub.wait_until_connected() - sub.assertMessageReceived() - assert self.kick_succeeded, "Dynup was not successful" - - def test_walkready(self): - self.one_run("walkready") - - def test_front(self): - self.one_run("front") - - def test_back(self): - self.one_run("back") - - def test_rise(self): - self.one_run("rise") - - def test_descend(self): - self.one_run("descend") - -if __name__ == '__main__': - from bitbots_test import run_rostests - run_rostests(DynupRunsTestCase) diff --git a/bitbots_hcm/CMakeLists.txt b/bitbots_hcm/CMakeLists.txt index 2afd6cc93..0bb80a6ea 100644 --- a/bitbots_hcm/CMakeLists.txt +++ b/bitbots_hcm/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(bitbots_docs REQUIRED) find_package(bitbots_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(humanoid_league_msgs REQUIRED) find_package(pybind11 REQUIRED) find_package(Python3 REQUIRED) find_package(PythonLibs REQUIRED) @@ -34,7 +33,6 @@ ament_target_dependencies(HCM bitbots_msgs builtin_interfaces geometry_msgs - humanoid_league_msgs rclcpp ros2_python_extension sensor_msgs @@ -55,14 +53,20 @@ install(DIRECTORY config install(DIRECTORY scripts/ USE_SOURCE_PERMISSIONS DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY bitbots_hcm/classifier DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY bitbots_hcm/hcm_dsd/actions DESTINATION share/${PROJECT_NAME}/hcm_dsd FILES_MATCHING PATTERN "*.py") -install(DIRECTORY bitbots_hcm/hcm_dsd/actions DESTINATION share/${PROJECT_NAME} FILES_MATCHING PATTERN "*.py") +install(DIRECTORY bitbots_hcm/hcm_dsd/decisions DESTINATION share/${PROJECT_NAME}/hcm_dsd FILES_MATCHING PATTERN "*.py") -install(DIRECTORY bitbots_hcm/hcm_dsd/decisions DESTINATION share/${PROJECT_NAME} FILES_MATCHING PATTERN "*.py") +install(FILES bitbots_hcm/hcm_dsd/hcm.dsd DESTINATION share/${PROJECT_NAME}/hcm_dsd) -install(FILES bitbots_hcm/hcm_dsd/hcm.dsd DESTINATION share/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(hcm_py_test test/pytest + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + ) +endif() ament_python_install_package(${PROJECT_NAME}) ament_package() diff --git a/bitbots_hcm/bitbots_hcm/classifier/classifier.pkl b/bitbots_hcm/bitbots_hcm/classifier/classifier.pkl deleted file mode 100644 index 1b27488ba..000000000 Binary files a/bitbots_hcm/bitbots_hcm/classifier/classifier.pkl and /dev/null differ diff --git a/bitbots_hcm/bitbots_hcm/classifier/scaler.pkl b/bitbots_hcm/bitbots_hcm/classifier/scaler.pkl deleted file mode 100644 index 19c3de243..000000000 Binary files a/bitbots_hcm/bitbots_hcm/classifier/scaler.pkl and /dev/null differ diff --git a/bitbots_hcm/bitbots_hcm/classifier/types.pkl b/bitbots_hcm/bitbots_hcm/classifier/types.pkl deleted file mode 100644 index 6715e9d1b..000000000 Binary files a/bitbots_hcm/bitbots_hcm/classifier/types.pkl and /dev/null differ diff --git a/bitbots_hcm/bitbots_hcm/fall_checker.py b/bitbots_hcm/bitbots_hcm/fall_checker.py deleted file mode 100644 index 80ea4611b..000000000 --- a/bitbots_hcm/bitbots_hcm/fall_checker.py +++ /dev/null @@ -1,190 +0,0 @@ -# -*- coding: utf8 -*- -import math -import numpy -from functools import reduce - -from rclpy.duration import Duration -from sensor_msgs.msg import Imu -import rclpy -from rclpy.node import Node - -from sklearn.base import BaseEstimator -from sklearn.metrics import accuracy_score - - -class FallChecker(BaseEstimator): - - def __init__(self, node, thresh_gyro_pitch=None, - thresh_gyro_roll=None, - thresh_orient_pitch=None, - thresh_orient_roll=None, - smoothing=None): - self.node = node - self.thresh_gyro_pitch = self.node.get_parameter("falling_thresh_gyro_pitch").get_parameter_value().double_value \ - if thresh_gyro_pitch is None else thresh_gyro_pitch - self.thresh_gyro_roll = self.node.get_parameter("falling_thresh_gyro_roll").get_parameter_value().double_value \ - if thresh_gyro_roll is None else thresh_gyro_roll - self.thresh_orient_pitch = math.radians(self.node.get_parameter("falling_thresh_orient_pitch").get_parameter_value().double_value) \ - if thresh_orient_pitch is None else thresh_orient_pitch - self.thresh_orient_roll = math.radians(self.node.get_parameter("falling_thresh_orient_roll").get_parameter_value().double_value) \ - if thresh_orient_roll is None else thresh_orient_roll - - self.smoothing = self.node.get_parameter("smooth_threshold").get_parameter_value().double_value if smoothing is None else smoothing - self.smoothing_list = [] - self.counter = 0 - self.last_result = 0 - - self.STABLE = 0 - self.FRONT = 1 - self.BACK = 2 - self.LEFT = 3 - self.RIGHT = 4 - - def update_reconfigurable_values(self, config, level): - # Dynamic Reconfigure - self.thresh_gyro_pitch = config["falling_thresh_gyro_pitch"] - self.thresh_gyro_roll = config["falling_thresh_gyro_roll"] - self.thresh_orient_pitch = math.radians(config["falling_thresh_orient_pitch"]) - self.thresh_orient_roll = math.radians(config["falling_thresh_orient_roll"]) - return config - - def check_falling(self, not_much_smoothed_gyro, quaternion): - """Checks if the robot is currently falling and in which direction. """ - # Convert quaternion to fused angles - fused_roll, fused_pitch, _ = self.fused_from_quat(quaternion) - - # setting the fall quantification function - roll_fall_quantification = self.calc_fall_quantification( - self.thresh_orient_roll, - self.thresh_gyro_roll, - fused_roll, - not_much_smoothed_gyro[0]) - - pitch_fall_quantification = self.calc_fall_quantification( - self.thresh_orient_pitch, - self.thresh_gyro_pitch, - fused_pitch, - not_much_smoothed_gyro[1]) - - if roll_fall_quantification + pitch_fall_quantification == 0: - result = self.STABLE - else: - # compare quantification functions - if pitch_fall_quantification > roll_fall_quantification: - # detect the falling direction - if fused_pitch < 0: - result = self.BACK - # detect the falling direction - else: - result = self.FRONT - else: - # detect the falling direction - if fused_roll < 0: - result = self.LEFT - # detect the falling direction - else: - result = self.RIGHT - - # Prune old elements from smoothing history - self.smoothing_list = list(filter( - lambda x: x[0] > self.node.get_clock().now() - Duration(seconds=self.smoothing), - self.smoothing_list)) - - # Add the current element - self.smoothing_list.append((self.node.get_clock().now(), result)) - - # List only including the results not the whole tuples - results_list = list(zip(*self.smoothing_list))[1] - - # Check if stable is not in the list otherwise say we are stable - # This smooths the output but prevents the output of stable when jittering between e.g. right and front - if self.STABLE in results_list: - result = self.STABLE - - return result - - def calc_fall_quantification(self, falling_threshold_orientation, falling_threshold_gyro, current_axis_euler, - current_axis__gyro): - # check if you are moving forward or away from the perpendicular position, by comparing the signs. - moving_more_upright = numpy.sign(current_axis_euler) != numpy.sign(current_axis__gyro) - - # Check if the orientation is over the point of no return threshold - over_point_of_no_return = abs(current_axis_euler) > falling_threshold_orientation - - # Calculate quantification if we are moving away from our upright position or if we are over the point of no return - if not moving_more_upright or over_point_of_no_return: - # calculatiung the orentation skalar for the threshold - skalar = max((falling_threshold_orientation - abs(current_axis_euler)) / falling_threshold_orientation, 0) - # checking if the rotation velocity is lower than the the threshold - if falling_threshold_gyro * skalar < abs(current_axis__gyro): - # returning the fall quantification function - return abs(current_axis__gyro) * (1 - skalar) - return 0 - - def fit(self, x, y): - # we have to do nothing, as we are not actually fitting any model - self.node.get_logger().warn("You can not train this type of classifier", once=True) - pass - - def score(self, X, y, sample_weight=None): - return accuracy_score(y, self.predict(X), sample_weight=sample_weight) - - def predict(self, x): - # only take gyro and orientation from data - y = [] - for entry in x: - prediction = self.check_falling(entry[3:6], entry[6:10]) - y.append(prediction) - return y - - def check_fallen(self, quaternion, not_much_smoothed_gyro): - """Check if the robot has fallen and is lying on the floor. Returns animation to play, if necessary.""" - - if numpy.mean(numpy.abs(not_much_smoothed_gyro)) >= 0.2: - return None - - # Convert quaternion to fused angles - fused_roll, fused_pitch, _ = self.fused_from_quat(quaternion) - - # Decides which side is facing downwards. - if fused_pitch > math.radians(60): - self.node.get_logger().info("FALLEN TO THE FRONT") - return self.FRONT - - if fused_pitch < math.radians(-60): - self.node.get_logger().info("FALLEN TO THE BACK") - return self.BACK - - if fused_roll > math.radians(60): - self.node.get_logger().info("FALLEN TO THE RIGHT") - return self.RIGHT - - if fused_roll < math.radians(-60): - self.node.get_logger().info("FALLEN TO THE LEFT") - return self.LEFT - - # If no side is facing downwards, the robot is not fallen yet. - return None - - def fused_from_quat(self, q): - # Fused yaw of Quaternion - fused_yaw = 2.0 * math.atan2(q[2], q[3]) # Output of atan2 is [-pi,pi], so this expression is in [-2*pi,2*pi] - if fused_yaw > math.pi: - fused_yaw -= 2 * math.pi # fused_yaw is now in[-2 * pi, pi] - if fused_yaw <= -math.pi: - fused_yaw += 2 * math.pi # fused_yaw is now in (-pi, pi] - - # Calculate the fused pitch and roll - stheta = 2.0 * (q[1] * q[3] - q[0] * q[2]) - sphi = 2.0 * (q[1] * q[2] + q[0] * q[3]) - if stheta >= 1.0: # Coerce stheta to[-1, 1] - stheta = 1.0 - elif stheta <= -1.0: - stheta = -1.0 - if sphi >= 1.0: # Coerce sphi to[-1, 1] - sphi = 1.0 - elif sphi <= -1.0: - sphi = -1.0 - fused_pitch = math.asin(stheta) - fused_roll = math.asin(sphi) - return fused_roll, fused_pitch, fused_yaw diff --git a/bitbots_hcm/bitbots_hcm/fall_classifier.py b/bitbots_hcm/bitbots_hcm/fall_classifier.py deleted file mode 100644 index 13500ab2b..000000000 --- a/bitbots_hcm/bitbots_hcm/fall_classifier.py +++ /dev/null @@ -1,113 +0,0 @@ -import pickle -import time - -from sklearn.preprocessing import StandardScaler -import transforms3d -from sensor_msgs.msg import JointState, Imu, Image -from geometry_msgs.msg import Point -import math -import numpy as np - -class FallClassifier: - - def __init__(self, path, smooth_threshold=10): - """We open a saved classifier which uses the scikit-learn library.""" - with open(path + "classifier.pkl", 'rb') as file: - self.classifier = pickle.load(file) - with open(path + "scaler.pkl", 'rb') as file: - self.scaler: StandardScaler = pickle.load(file) - with open(path + "types.pkl", 'rb') as file: - self.types = pickle.load(file) - - # print(F'{self.classifier} {self.types}') - - self.counter = 0 - self.last_prediction = 0 - self.smooth_threshold = smooth_threshold - - def classify(self, imu, joint_state, cop_l, cop_r): - start_time = time.time() - data = get_data_from_msgs(imu, joint_state, cop_l, cop_r, - imu_raw=self.types['imu_raw'], - imu_orient=self.types['imu_orient'], joint_states=self.types['joint_states'], - imu_fused=self.types['imu_fused'], cop=self.types['cop']) - scaled_date = self.scaler.transform([data]) - result = self.classifier.predict(scaled_date) - # print((time.time() - start_time) * 1000) - return result[0] - - def smooth_classify(self, imu, joint_state, cop_l, cop_r): - """ Only predict a fall if we got same result more than smooth_threshold times straight. """ - prediction = self.classify(imu, joint_state, cop_l, cop_r) - if prediction == self.last_prediction and prediction != 0: - self.counter += 1 - if self.counter > self.smooth_threshold: - result = prediction - else: - result = 0 - else: - self.counter = 0 - result = 0 - self.last_prediction = prediction - return result - - -def get_data_from_msgs(imu_msg, joint_state_msg, cop_l_msg, cop_r_msg, imu_raw=True, imu_orient=True, joint_states=True, - imu_fused=True, cop=True): - data = [] - if imu_raw: - data.append(imu_msg.linear_acceleration.x) - data.append(imu_msg.linear_acceleration.y) - data.append(imu_msg.linear_acceleration.z) - data.append(imu_msg.angular_velocity.x) - data.append(imu_msg.angular_velocity.y) - data.append(imu_msg.angular_velocity.z) - if imu_orient: - euler = transforms3d.euler.quat2euler([imu_msg.orientation.w, imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z]) - data.append(euler[0]) - data.append(euler[1]) - data.append(euler[2]) - - if joint_states: - for i in range(len(joint_state_msg.name)): - # only add leg joints - if joint_state_msg.name[i] in ["RHipPitch", "RHipRoll", "RKnee", "RAnklePitch", "RAnkleRoll", - "LHipPitch", "LHipRoll", "LKnee", "LAnklePitch", "LAnkleRoll"]: - data.append(joint_state_msg.effort[i]) - if imu_fused: - fused_rpy = fused_from_quat(imu_msg.orientation) - data.append(fused_rpy[0]) - data.append(fused_rpy[1]) - - if cop: - data.append(cop_l_msg.point.x) - data.append(cop_l_msg.point.y) - data.append(cop_r_msg.point.x) - data.append(cop_r_msg.point.y) - return data - - -# Python version of this code https://github.com/AIS-Bonn/rot_conv_lib/blob/master/src/rot_conv.cpp -def fused_from_quat(q): - # Fused yaw of Quaternion - fused_yaw = 2.0 * math.atan2(q.z, q.w) # Output of atan2 is [-pi,pi], so this expression is in [-2*pi,2*pi] - if fused_yaw > math.pi: - fused_yaw -= 2 * math.pi # fused_yaw is now in[-2 * pi, pi] - if fused_yaw <= -math.pi: - fused_yaw += 2 * math.pi # fused_yaw is now in (-pi, pi] - - # Calculate the fused pitch and roll - stheta = 2.0 * (q.y * q.w - q.x * q.z) - sphi = 2.0 * (q.y * q.z + q.x * q.w) - if stheta >= 1.0: # Coerce stheta to[-1, 1] - stheta = 1.0 - elif stheta <= -1.0: - stheta = -1.0 - if sphi >= 1.0: # Coerce sphi to[-1, 1] - sphi = 1.0 - elif sphi <= -1.0: - sphi = -1.0 - fused_pitch = math.asin(stheta) - fused_roll = math.asin(sphi) - - return fused_roll, fused_pitch, fused_yaw diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py index e69de29bb..997192bdf 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py @@ -0,0 +1,13 @@ +from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard + +from dynamic_stack_decider.abstract_action_element import \ + AbstractActionElement + + +class AbstractHCMActionElement(AbstractActionElement): + """ + AbstractHCMActionElement with a hcm blackboard as its blackboard + """ + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + self.blackboard: HcmBlackboard diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/cancel_goals.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/cancel_goals.py index 28235fbbd..d01ce52ad 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/cancel_goals.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/cancel_goals.py @@ -1,13 +1,13 @@ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - -class CancelGoals(AbstractActionElement): +class CancelGoals(AbstractHCMActionElement): + """ + Cancels all animation, dynup and move_base goals + """ def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard def perform(self): if self.blackboard.animation_action_current_goal is not None: @@ -21,6 +21,5 @@ def perform(self): except: pass - self.blackboard.cancel_move_base_goal() + self.blackboard.cancel_path_planning() self.pop() - diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py index e3cec4a2a..ee01ed493 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_power.py @@ -1,25 +1,22 @@ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement from std_srvs.srv import SetBool -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - -class AbstractChangeMotorPower(AbstractActionElement): +class AbstractChangeMotorPower(AbstractHCMActionElement): """ Switches motor power using the service call of the hardware interface. """ def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard if not self.blackboard.visualization_active and not self.blackboard.simulation_active: # In visualization and simulation, we cannot disable motors try: - blackboard.motor_switch_service.wait_for_service(timeout_sec=10) + self.blackboard.motor_switch_service.wait_for_service(timeout_sec=10) except: self.blackboard.node.get_logger().warn("HCM waiting for switch power service") - blackboard.motor_switch_service.wait_for_service() + self.blackboard.motor_switch_service.wait_for_service() def perform(self, reevaluate=False): raise NotImplementedError diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/falling_poses.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/falling_poses.py deleted file mode 100644 index ff4b72c24..000000000 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/falling_poses.py +++ /dev/null @@ -1,218 +0,0 @@ -from math import radians - -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard -from rclpy.duration import Duration - -from bitbots_msgs.msg import JointCommand -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - - -def parse_dict_to_msg(joint_dict: dict, time_stamp): - names = [] - positions = [] - for name, value in joint_dict.items(): - names.append(name) - positions.append(radians(value)) - msg = JointCommand() - msg.joint_names = names - msg.positions = positions - msg.velocities = len(names) * [-1.0] - msg.accelerations = len(names) * [-1.0] - msg.max_currents = len(names) * [-1.0] - msg.header.stamp = time_stamp - return msg - - -class AbstractFallingPose(AbstractActionElement): - - def __init__(self, blackboard: HcmBlackboard, dsd, parameters: dict = None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - self.first_perform = True - self.start_time = blackboard.node.get_clock().now() - self.duration = parameters.get('duration', 1.0) - - def perform(self, reevaluate=False): - self.do_not_reevaluate() - self.blackboard.joint_pub.publish(self.falling_pose) - # wait some time - if self.blackboard.node.get_clock().now() - self.start_time > Duration(seconds=self.duration): - self.pop() - - def get_side_fall_head_pan(self): - head_index = self.blackboard.current_joint_state.name.index("HeadPan") - current_head_pan = self.blackboard.current_joint_state.position[head_index] - if current_head_pan > 0: - return 90 - else: - return -90 - - -class FallingPoseFront(AbstractFallingPose): - - def __init__(self, blackboard: HcmBlackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - joint_dict = { - "HeadPan": 0.0, - "HeadTilt": 45.0, - "LAnklePitch": -36.0, - "LAnkleRoll": 4.0, - "LElbow": 45.0, - "LHipPitch": -11.0, - "LHipRoll": 4.0, - "LHipYaw": 6.0, - "LKnee": 13.0, - "LShoulderPitch": 90.0, - "LShoulderRoll": 0.0, - "RAnklePitch": 36.0, - "RAnkleRoll": -4.0, - "RElbow": -45.0, - "RHipPitch": 11.0, - "RHipRoll": -4.0, - "RHipYaw": 6.0, - "RKnee": -13.0, - "RShoulderPitch": -90.0, - "RShoulderRoll": 0.0 - } - self.falling_pose = parse_dict_to_msg(joint_dict, self.blackboard.node.get_clock().now().to_msg()) - - -class FallingPoseBack(AbstractFallingPose): - - def __init__(self, blackboard: HcmBlackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - joint_dict = { - "HeadPan": 0.0, - "HeadTilt": 0.0, - "LAnklePitch": -29.0, - "LAnkleRoll": 0.0, - "LElbow": 47.0, - "LHipPitch": 51.0, - "LHipRoll": 0.0, - "LHipYaw": 0.0, - "LKnee": 64.0, - "LShoulderPitch": 1.0, - "LShoulderRoll": 0.0, - "RAnklePitch": 28.0, - "RAnkleRoll": -4.0, - "RElbow": -45.0, - "RHipPitch": -50.0, - "RHipRoll": -1.0, - "RHipYaw": 1.0, - "RKnee": -61.0, - "RShoulderPitch": 0.0, - "RShoulderRoll": 0.0 - } - self.falling_pose = parse_dict_to_msg(joint_dict, self.blackboard.node.get_clock().now().to_msg()) - - -class FallingPoseLeft(AbstractFallingPose): - - def __init__(self, blackboard: HcmBlackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - joint_dict = { - "HeadPan": self.get_side_fall_head_pan(), - "HeadTilt": 0.0, - "LAnklePitch": -26.0, - "LAnkleRoll": 4.0, - "LElbow": 44.0, - "LHipPitch": 27.0, - "LHipRoll": 4.0, - "LHipYaw": -1.0, - "LKnee": 58.0, - "LShoulderPitch": -2.0, - "LShoulderRoll": 0.0, - "RAnklePitch": 26.0, - "RAnkleRoll": -4.0, - "RElbow": -42.0, - "RHipPitch": -27.0, - "RHipRoll": -4.0, - "RHipYaw": 1.0, - "RKnee": -58.0, - "RShoulderPitch": 6.0, - "RShoulderRoll": 0.0 - } - self.falling_pose = parse_dict_to_msg(joint_dict, self.blackboard.node.get_clock().now().to_msg()) - - -class FallingPoseRight(AbstractFallingPose): - - def __init__(self, blackboard: HcmBlackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - joint_dict = { - "HeadPan": self.get_side_fall_head_pan(), - "HeadTilt": 0.0, - "LAnklePitch": -26.0, - "LAnkleRoll": 4.0, - "LElbow": 44.0, - "LHipPitch": 27.0, - "LHipRoll": 4.0, - "LHipYaw": -1.0, - "LKnee": 58.0, - "LShoulderPitch": -2.0, - "LShoulderRoll": 0.0, - "RAnklePitch": 26.0, - "RAnkleRoll": -4.0, - "RElbow": -42.0, - "RHipPitch": -27.0, - "RHipRoll": -4.0, - "RHipYaw": 1.0, - "RKnee": -58.0, - "RShoulderPitch": 6.0, - "RShoulderRoll": 0.0 - } - self.falling_pose = parse_dict_to_msg(joint_dict, self.blackboard.node.get_clock().now().to_msg()) - -class TurnRightToBack(AbstractFallingPose): - def __init__(self, blackboard: HcmBlackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - joint_dict = { - "HeadPan": 0, - "HeadTilt": 0, - "LAnklePitch": -26, - "LAnkleRoll": 4, - "LElbow": 45, - "LHipPitch": 27, - "LHipRoll": 4, - "LHipYaw": -1, - "LKnee": 58, - "LShoulderPitch": 0, - "LShoulderRoll": 0, - "RAnklePitch": 26, - "RAnkleRoll": -4, - "RElbow": -45, - "RHipPitch": -27, - "RHipRoll": -4, - "RHipYaw": 43, - "RKnee": -58, - "RShoulderPitch": 0, - "RShoulderRoll": 0 - } - self.falling_pose = parse_dict_to_msg(joint_dict, self.blackboard.node.get_clock().now().to_msg()) - -class TurnLeftToBack(AbstractFallingPose): - def __init__(self, blackboard: HcmBlackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - joint_dict = { - "HeadPan": 0, - "HeadTilt": 0, - "LAnklePitch": -26, - "LAnkleRoll": 4, - "LElbow": 45, - "LHipPitch": 27, - "LHipRoll": 4, - "LHipYaw": -43, - "LKnee": 58, - "LShoulderPitch": 0, - "LShoulderRoll": 0, - "RAnklePitch": 26, - "RAnkleRoll": -4, - "RElbow": -45, - "RHipPitch": -27, - "RHipRoll": -4, - "RHipYaw": 1, - "RKnee": -58, - "RShoulderPitch": 0, - "RShoulderRoll": 0 - } - self.falling_pose = parse_dict_to_msg(joint_dict, self.blackboard.node.get_clock().now().to_msg()) diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py index a98f64a12..5c15353b0 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py @@ -1,21 +1,19 @@ -from action_msgs.msg import GoalStatus -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from abc import abstractmethod, ABC import rclpy -from bitbots_msgs.action import Dynup -from dynamic_stack_decider.abstract_action_element import AbstractActionElement -from humanoid_league_msgs.action import PlayAnimation -from humanoid_league_msgs.msg import RobotControlState +from action_msgs.msg import GoalStatus +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement +from bitbots_msgs.action import Dynup, PlayAnimation -class AbstractPlayAnimation(AbstractActionElement): + +class AbstractPlayAnimation(AbstractHCMActionElement, ABC): """ Abstract class to create actions for playing animations """ def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard self.first_perform = True def perform(self, reevaluate=False): @@ -29,6 +27,7 @@ def perform(self, reevaluate=False): # try to start animation sucess = self.start_animation(anim) + # if we fail, we need to abort this action if not sucess: self.blackboard.node.get_logger().error("Could not start animation. Will abort play animation action!") @@ -41,6 +40,7 @@ def perform(self, reevaluate=False): # we are finished playing this animation return self.pop() + @abstractmethod def chose_animation(self): # this is what has to be implemented returning the animation to play raise NotImplementedError @@ -59,10 +59,10 @@ def start_animation(self, anim): self.blackboard.node.get_logger().warn("Tried to play an animation with an empty name!") return False first_try = self.blackboard.animation_action_client.wait_for_server( - timeout_sec=self.blackboard.node.get_parameter('hcm.anim_server_wait_time').get_parameter_value().double_value) + timeout_sec=self.blackboard.node.get_parameter('hcm.anim_server_wait_time').value) if not first_try: server_running = False - while not server_running and not self.blackboard.shut_down_request and rclpy.ok(): + while not server_running and rclpy.ok(): self.blackboard.node.get_logger().warn( "Animation Action Server not running! Motion can not work without animation action server. " "Will now wait until server is accessible!", @@ -76,100 +76,65 @@ def start_animation(self, anim): goal = PlayAnimation.Goal() goal.animation = anim goal.hcm = True # the animation is from the hcm - self.blackboard.animation_action_current_goal = self.blackboard.animation_action_client.send_goal_async(goal, feedback_callback=self.blackboard.last_kick_feedback_callback) + self.blackboard.animation_action_current_goal = self.blackboard.animation_action_client.send_goal_async( + goal, feedback_callback=self.animation_feedback_cb) return True - def animation_finished(self): - return self.blackboard.animation_action_current_goal.cancelled() or self.blackboard.animation_action_current_goal.done() - -class PlayAnimationStandUpFront(AbstractPlayAnimation): - def chose_animation(self): - self.blackboard.current_state = RobotControlState.GETTING_UP - self.blackboard.node.get_logger().info("PLAYING STAND UP FRONT ANIMATION") - return self.blackboard.stand_up_front_animation - + def animation_feedback_cb(self, msg): + feedback: PlayAnimation.Feedback = msg.feedback + self.publish_debug_data("Animation Percent Done", str(feedback.percent_done)) -class PlayAnimationStandUpBack(AbstractPlayAnimation): - def chose_animation(self): - self.blackboard.current_state = RobotControlState.GETTING_UP - self.blackboard.node.get_logger().info("PLAYING STAND UP BACK ANIMATION") - return self.blackboard.stand_up_back_animation - - -class PlayAnimationStandUpLeft(AbstractPlayAnimation): - def chose_animation(self): - self.blackboard.current_state = RobotControlState.GETTING_UP - self.blackboard.node.get_logger().info("PLAYING STAND UP LEFT ANIMATION") - return self.blackboard.stand_up_left_animation - - -class PlayAnimationStandUpRight(AbstractPlayAnimation): - def chose_animation(self): - self.blackboard.current_state = RobotControlState.GETTING_UP - self.blackboard.node.get_logger().info("PLAYING STAND UP RIGHT ANIMATION") - return self.blackboard.stand_up_right_animation + def animation_finished(self): + return (self.blackboard.animation_action_current_goal.done() and self.blackboard.animation_action_current_goal.result().status == GoalStatus.STATUS_SUCCEEDED) \ + or self.blackboard.animation_action_current_goal.cancelled() class PlayAnimationFallingLeft(AbstractPlayAnimation): def chose_animation(self): self.blackboard.node.get_logger().info("PLAYING FALLING LEFT ANIMATION") - return self.blackboard.falling_animation_left + return self.blackboard.animation_name_falling_left class PlayAnimationFallingRight(AbstractPlayAnimation): def chose_animation(self): self.blackboard.node.get_logger().info("PLAYING FALLING RIGHT ANIMATION") - return self.blackboard.falling_animation_right + return self.blackboard.animation_name_falling_right class PlayAnimationFallingFront(AbstractPlayAnimation): def chose_animation(self): self.blackboard.node.get_logger().info("PLAYING FALLING FRONT ANIMATION") - return self.blackboard.falling_animation_front + return self.blackboard.animation_name_falling_front class PlayAnimationFallingBack(AbstractPlayAnimation): def chose_animation(self): self.blackboard.node.get_logger().info("PLAYING FALLING BACK ANIMATION") - return self.blackboard.falling_animation_back - - -class PlayAnimationStopped(AbstractPlayAnimation): - def chose_animation(self): - return self.blackboard.stop_animation + return self.blackboard.animation_name_falling_back -class PlayAnimationWalkready(AbstractPlayAnimation): +class PlayAnimationTurningBackLeft(AbstractPlayAnimation): def chose_animation(self): - return self.blackboard.walkready_animation - - -class PlayAnimationSitDown(AbstractPlayAnimation): - def chose_animation(self): - return self.blackboard.sit_down_animation - + self.blackboard.node.get_logger().info("LYING ON THE LEFT SIDE AND TURNING TO THE BACK TO GET UP") + return self.blackboard.animation_name_turning_back_left -class PlayAnimationMotorOff(AbstractPlayAnimation): +class PlayAnimationTurningBackRight(AbstractPlayAnimation): def chose_animation(self): - return self.blackboard.motor_off_animation + self.blackboard.node.get_logger().info("LYING ON THE RIGHT SIDE AND TURNING TO THE BACK TO GET UP") + return self.blackboard.animation_name_turning_back_right -class PlayAnimationDynup(AbstractActionElement): +class PlayAnimationDynup(AbstractHCMActionElement): def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard self.direction = parameters.get('direction') self.first_perform = True - # A parameter 'initial' is passed when dynup is called during the startup phase, - # in this case we do not want to set the state to GETTING_UP. - initial = parameters.get('initial', False) - if not initial: - self.blackboard.current_state = RobotControlState.GETTING_UP - def perform(self, reevaluate=False): # deactivate falling since it will be wrongly detected self.do_not_reevaluate() + + # We only want to execute this once if self.first_perform: # get the animation that should be played # defined by implementations of this abstract class @@ -190,19 +155,17 @@ def perform(self, reevaluate=False): def start_animation(self): """ - This will NOT wait by itself. You have to check - animation_finished() - by yourself. + This will NOT wait by itself. You have to check animation_finished() by yourself. :return: """ first_try = self.blackboard.dynup_action_client.wait_for_server( - timeout_sec=self.blackboard.node.get_parameter('hcm.anim_server_wait_time').get_parameter_value().double_value) + timeout_sec=self.blackboard.node.get_parameter('hcm.anim_server_wait_time').value) if not first_try: server_running = False - while not server_running and not self.blackboard.shut_down_request and rclpy.ok(): + while not server_running and rclpy.ok(): self.blackboard.node.get_logger().warn( - "Dynup Action Server not running! Dynup cannot work without dynup server!" + "Dynup Action Server not running! Dynup cannot work without dynup server! " "Will now wait until server is accessible!", throttle_duration_sec=10.0) server_running = self.blackboard.dynup_action_client.wait_for_server(timeout_sec=1) @@ -211,11 +174,17 @@ def start_animation(self): else: self.blackboard.node.get_logger().warn("Dynup server did not start.") return False + goal = Dynup.Goal() goal.direction = self.direction - self.blackboard.dynup_action_current_goal = self.blackboard.dynup_action_client.send_goal_async(goal) + self.blackboard.dynup_action_current_goal = self.blackboard.dynup_action_client.send_goal_async( + goal, feedback_callback=self.animation_feedback_cb) return True + def animation_feedback_cb(self, msg): + feedback: Dynup.Feedback = msg.feedback + self.publish_debug_data("Dynup Percent Done", str(feedback.percent_done)) + def animation_finished(self): return (self.blackboard.dynup_action_current_goal.done() and self.blackboard.dynup_action_current_goal.result().status == GoalStatus.STATUS_SUCCEEDED) \ or self.blackboard.dynup_action_current_goal.cancelled() diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/set_foot_zero.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/set_foot_zero.py index 0081e1d50..5241870b1 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/set_foot_zero.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/set_foot_zero.py @@ -1,21 +1,22 @@ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - -class SetFootZero(AbstractActionElement): +class SetFootZero(AbstractHCMActionElement): def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard self.first_perform = True def perform(self, reevaluate=False): + # Just to be sure, we do not want to reevaluate + self.do_not_reevaluate() + + # We only want to execute this once if self.first_perform: # Executing this once is sufficient self.first_perform = False try: self.blackboard.foot_zero_service.wait_for_service(0.5) - self.blackboard.foot_zero_service() + self.blackboard.foot_zero_service.call_async({}) except: self.blackboard.node.get_logger().warn("No foot zeroing service accessible, will not reset sensors") diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py new file mode 100644 index 000000000..63f6eb5dd --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py @@ -0,0 +1,93 @@ +from abc import abstractmethod, ABC + +from bitbots_msgs.msg import RobotControlState + +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement + + +class AbstractRobotState(AbstractHCMActionElement, ABC): + """ + Abstract class to create actions which just stay on top of the stack + and set the robot state accordingly. + """ + + @abstractmethod + def get_state(self) -> RobotControlState: + """ + Returns the state which should be set. This will be implemented by the subclasses. + """ + raise NotImplementedError() + + def perform(self, reevaluate=False): + # Just to be sure, we do not reevaluate + self.do_not_reevaluate() + # Set the state + self.blackboard.current_state = self.get_state() + # Our job is done, we can pop + self.pop() + + + +class RobotStateStartup(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.STARTUP + + +class RobotStateControllable(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.CONTROLLABLE + + +class RobotStateWalking(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.WALKING + + +class RobotStateAnimationRunning(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.ANIMATION_RUNNING + + +class RobotStatePickedUp(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.PICKED_UP + + +class RobotStateFalling(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.FALLING + + +class RobotStateFallen(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.FALLEN + + +class RobotStateGettingUp(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.GETTING_UP + + +class RobotStatePenalty(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.PENALTY + + +class RobotStateRecord(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.RECORD + + +class RobotStateKicking(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.KICKING + + +class RobotStateHardwareProblem(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.HARDWARE_PROBLEM + + +class RobotStateMotorOff(AbstractRobotState): + def get_state(self) -> RobotControlState: + return RobotControlState.MOTOR_OFF diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stay.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stay.py deleted file mode 100644 index de5c63807..000000000 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stay.py +++ /dev/null @@ -1,58 +0,0 @@ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard - -from dynamic_stack_decider.abstract_action_element import AbstractActionElement -from humanoid_league_msgs.msg import RobotControlState - - -class AbstractStay(AbstractActionElement): - """ - Abstract class to create actions which just stay on top of the stack. - This can be used to stay in a certain state till some precondition changes. - Implementations can be used to change the name - """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - - def perform(self): - # just do nothing - return - - -class StayControlable(AbstractStay): - def perform(self): - self.blackboard.current_state = RobotControlState.CONTROLLABLE - - -class StayWalking(AbstractStay): - pass - - -class StayAnimationRunning(AbstractStay): - pass - - -class StayPickedUp(AbstractStay): - pass - - -class StayMotorsOff(AbstractStay): - pass - - -class StayStopped(AbstractStay): - def perform(self): - self.blackboard.current_state = RobotControlState.PENALTY - - -class StayRecord(AbstractStay): - pass - - -class StayShutDown(AbstractStay): - def perform(self): - self.blackboard.current_state = RobotControlState.HCM_OFF - -class StayKicking(AbstractStay): - pass diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stop_walking.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stop_walking.py index 71b626473..feffaf860 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stop_walking.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/stop_walking.py @@ -1,45 +1,13 @@ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement from geometry_msgs.msg import Twist -from dynamic_stack_decider.abstract_action_element import AbstractActionElement -from humanoid_league_msgs.msg import RobotControlState - -class StopWalking(AbstractActionElement): +class StopWalking(AbstractHCMActionElement): """ Stop the walking """ - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - - def perform(self, reevaluate=False): - if self.blackboard.current_time.to_sec() - self.blackboard.last_walking_goal_time.to_sec() < 0.1: - msg = Twist() - msg.linear.x = 0.0 - msg.linear.y = 0.0 - msg.angular.z = 0.0 - msg.angular.x = -1.0 - self.blackboard.walk_pub.publish(msg) - else: - self.pop() - - -class ForceStopWalking(AbstractActionElement): - """ - Stop the walking and set the state to penalty - """ - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - def perform(self, reevaluate=False): msg = Twist() - msg.linear.x = 0.0 - msg.linear.y = 0.0 - msg.angular.z = 0.0 msg.angular.x = -1.0 self.blackboard.walk_pub.publish(msg) - self.blackboard.current_state = RobotControlState.PENALTY - # We can pop immediately because the state is PENALTY on no walking messages will be passed self.pop() diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait.py index 965fcf562..75859cba3 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait.py @@ -7,31 +7,31 @@ Just waits for something (i.e. that preconditions will be fullfilled) """ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - -class Wait(AbstractActionElement): +class Wait(AbstractHCMActionElement): """ - This action waits a specified time before it pops itself + This action does nothing. If a time is given, it will wait for that time before it pops itself. """ def __init__(self, blackboard, dsd, parameters=None): """ :param parameters['time']: Time to wait in seconds """ - super().__init__(blackboard, dsd) - self.blackboard: HcmBlackboard - self.time = float(self.blackboard.node.get_clock().now().seconds_nanoseconds()[0] \ - + self.blackboard.node.get_clock().now().seconds_nanoseconds()[1]/1e9) \ - + float(parameters['time']) + super().__init__(blackboard, dsd, parameters) + self.duration = parameters.get("time", None) + self.start_time = self.blackboard.node.get_clock().now().nanoseconds / 1e9 def perform(self, reevaluate=False): """ Only pop when the wait-time has elapsed """ - if self.time < float(self.blackboard.node.get_clock().now().seconds_nanoseconds()[0] \ - + self.blackboard.node.get_clock().now().seconds_nanoseconds()[1]/1e9): + # Return directly if we want to wait infinitely + if self.duration is None: + return + + # Pop if the time has elapsed + if self.blackboard.node.get_clock().now().nanoseconds / 1e9 > self.start_time + self.duration: self.pop() diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait_for.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait_for.py index 6045b2df9..cc7bc5df9 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait_for.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/wait_for.py @@ -1,56 +1,31 @@ -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - -class WaitForIMUStartup(AbstractActionElement): +class WaitForIMUStartup(AbstractHCMActionElement): """ Waits for the IMU to connect and does not complain as we are still in start up. """ + pass - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - - def perform(self, reevaluate=False): - pass - - -class WaitForIMU(AbstractActionElement): +class WaitForIMU(AbstractHCMActionElement): """ Waits for the IMU to connect and publishes warnings while doing so """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - def perform(self, reevaluate=False): self.blackboard.node.get_logger().warn("HCM gets no IMU data. Waiting for IMU to connect.", throttle_duration_sec=10) -class WaitForPressureStartup(AbstractActionElement): +class WaitForPressureStartup(AbstractHCMActionElement): """ Waits for the pressure sensors to connect and not complain since we are still starting up. """ + pass - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - - def perform(self, reevaluate=False): - pass - -class WaitForPressure(AbstractActionElement): +class WaitForPressure(AbstractHCMActionElement): """ Waits for the pressure sensors to connect and publishes warnings while doing so """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - def perform(self, reevaluate=False): self.blackboard.node.get_logger().warn( "HCM gets no correct pressure data. Waiting for pressure sensors to connect.\n" @@ -61,28 +36,17 @@ def perform(self, reevaluate=False): "set the visualization_active parameter to True.", throttle_duration_sec=30) -class WaitForMotorStartup(AbstractActionElement): +class WaitForMotorStartup(AbstractHCMActionElement): """ Waits for the motors on startup without complaining if it takes a moment. """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - - def perform(self, reevaluate=False): - pass + pass -class WaitForMotors(AbstractActionElement): +class WaitForMotors(AbstractHCMActionElement): """ Waits for the motors to connect and publishes warnings while doing so """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - def perform(self, reevaluate=False): self.blackboard.node.get_logger().warn( "HCM gets no data from the motors (/joint_states). Waiting for the motors to " diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py index e69de29bb..b08ac2789 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py @@ -0,0 +1,13 @@ +from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard + +from dynamic_stack_decider.abstract_decision_element import \ + AbstractDecisionElement + + +class AbstractHCMDecisionElement(AbstractDecisionElement): + """ + AbstractHCMDecisionElement with a hcm blackboard as its blackboard + """ + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + self.blackboard: HcmBlackboard diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py new file mode 100644 index 000000000..05690041c --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py @@ -0,0 +1,32 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement + + +class RecordAnimation(AbstractHCMDecisionElement): + """ + Decides if the robot is currently recording animations + """ + + def perform(self, reevaluate=False): + if self.blackboard.record_active: + return "RECORD_ACTIVE" + else: + # robot is not recording + return "FREE" + + def get_reevaluate(self): + return True + + +class PlayingExternalAnimation(AbstractHCMDecisionElement): + """ + Decides if the robot is currently wants to play or plays an animation + """ + + def perform(self, reevaluate=False): + if self.blackboard.external_animation_running: + return "ANIMATION_RUNNING" + else: + return "FREE" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py new file mode 100644 index 000000000..6f4e1a492 --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py @@ -0,0 +1,165 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement +from bitbots_msgs.msg import RobotControlState + + +class CheckMotors(AbstractHCMDecisionElement): + """ + Checks if we are getting information from the motors. + Since the HCM is not able to work without motor connection, we will stop if there are no values. + Needs to be checked before other sensors, since they also need the power to be able to response + """ + + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + self.had_problem = False + + def perform(self, reevaluate=False): + self.clear_debug_data() + + # we check if the joint state values are actually changing, since the joint_state controller will publish the same message + # even if there is no connection anymore. But we don't want to go directly to hardware error if we just + # have a small break, since this can happen often due to loose cabling + if self.blackboard.previous_joint_state is not None and self.blackboard.current_joint_state is not None \ + and (self.blackboard.previous_joint_state.effort != self.blackboard.current_joint_state.effort + or self.blackboard.previous_joint_state.position != self.blackboard.current_joint_state.position) \ + and not self.blackboard.servo_diag_error: + self.blackboard.last_different_joint_state_time = self.blackboard.node.get_clock().now() + + if self.blackboard.visualization_active: + # we will have no problems with hardware in visualization + return "OKAY" + + if self.blackboard.simulation_active: + # Some simulators will give the exact same joint messages, which could look like errors, + # as the real world ros controller will always publish the same message if there is no connection + # so we will just the check if the message is changing in simulation + if self.blackboard.current_joint_state is None: + return "MOTORS_NOT_STARTED" + else: + return "OKAY" + + if self.blackboard.servo_overload: + return "OVERLOAD" + + # Check if we get no messages or always the exact same + if self.blackboard.last_different_joint_state_time is None or self.blackboard.node.get_clock().now().nanoseconds - \ + self.blackboard.last_different_joint_state_time.nanoseconds > self.blackboard.motor_timeout_duration * 1e9: + # Check if the motors have power + if self.blackboard.is_power_on: + # If we are currently in startup phase, we will wait a bit before we complain + if (self.blackboard.current_state == RobotControlState.STARTUP and + self.blackboard.node.get_clock().now().nanoseconds - self.blackboard.start_time.nanoseconds < 10 * 1e9): + # we are still in startup phase, just wait and dont complain + return "MOTORS_NOT_STARTED" + else: + # tell that we have a hardware problem + self.had_problem = True + # wait for motors to connect + return "PROBLEM" + else: + # we have to turn the motors on + return "TURN_ON" + + if self.had_problem: + # had problem before, just tell that this is solved now + self.blackboard.node.get_logger().info("Motors are now connected. Will resume.") + self.had_problem = False + + # motors are on and we can continue + return "OKAY" + + def get_reevaluate(self): + return True + + +class CheckIMU(AbstractHCMDecisionElement): + """ + Checks if we are getting information from the IMU. + Since the HCM can not detect falls without it, we will shut everything down if we dont have an imu. + """ + + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + self.had_problem = False + + def perform(self, reevaluate=False): + if self.blackboard.visualization_active: + # In visualization, we do not have an IMU. Therefore, return OKAY to ignore that. + return "OKAY" + + # we will get always the same message if there is no connection, so check if it differs + if self.blackboard.previous_imu_msg is not None and self.blackboard.imu_msg is not None \ + and not self.blackboard.previous_imu_msg.orientation == self.blackboard.imu_msg.orientation \ + and not self.blackboard.imu_diag_error: + self.blackboard.last_different_imu_state_time = self.blackboard.node.get_clock().now() + + if self.blackboard.simulation_active: + # Some simulators will give exact same IMU messages which look like errors, so ignore this case + if self.blackboard.imu_msg is None: + return "IMU_NOT_STARTED" + else: + return "OKAY" + + if self.blackboard.previous_imu_msg is None or (self.blackboard.node.get_clock().now().nanoseconds - \ + self.blackboard.last_different_imu_state_time.nanoseconds > self.blackboard.imu_timeout_duration * 1e9): + if self.blackboard.current_state == RobotControlState.STARTUP and self.blackboard.node.get_clock().now().nanoseconds - \ + self.blackboard.start_time.nanoseconds < 10 * 1e9: + # wait for the IMU to start + return "IMU_NOT_STARTED" + else: + self.had_problem = True + return "PROBLEM" + + if self.had_problem: + # had problem before, just tell that this is solved now + self.blackboard.node.get_logger().info("IMU is now connected. Will resume.") + self.had_problem = False + + return "OKAY" + + def get_reevaluate(self): + return True + + +class CheckPressureSensor(AbstractHCMDecisionElement): + """ + Checks connection to pressure sensors. + """ + + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + self.had_problem = False + + def perform(self, reevaluate=False): + if self.blackboard.visualization_active: + # no pressure sensors is visualization, but thats okay + return "OKAY" + + if not self.blackboard.pressure_sensors_installed: + # no pressure sensors installed, no check necessary + return "OKAY" + + # Check if we get no messages due to an error or always the exact same one (no connection) + if not self.blackboard.pressure_diag_error and not self.blackboard.previous_pressures == self.blackboard.pressures: + self.blackboard.last_different_pressure_state_time = self.blackboard.node.get_clock().now() + + # Check if we get no messages for a while + if self.blackboard.last_different_pressure_state_time is None or self.blackboard.node.get_clock().now().nanoseconds - \ + self.blackboard.last_different_pressure_state_time.nanoseconds > 0.1 * 1e9: + # Check if we are in the startup phase (not too long tho) + if self.blackboard.current_state == RobotControlState.STARTUP and self.blackboard.node.get_clock().now().nanoseconds - \ + self.blackboard.start_time.nanoseconds < 10 * 1e9: + # wait for the pressure sensors to start + return "PRESSURE_NOT_STARTED" + else: + return "PROBLEM" + + if self.had_problem: + # had problem before, just tell that this is solved now + self.blackboard.node.get_logger().info("Pressure sensors are now connected. Will resume.") + self.had_problem = False + + return "OKAY" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/decisions.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/decisions.py deleted file mode 100644 index 9a1b3e6f3..000000000 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/decisions.py +++ /dev/null @@ -1,468 +0,0 @@ -import math - -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard -from humanoid_league_speaker.speaker import speak -from rclpy.time import Duration, Time - -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement -from humanoid_league_msgs.msg import RobotControlState - - -class AbstractHCMDecisionElement(AbstractDecisionElement): - """ - AbstractHCMDecisionElement with a hcm blackboard as its blackboard - """ - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.blackboard: HcmBlackboard - - -class StartHCM(AbstractHCMDecisionElement): - """ - Initializes HCM. - """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.is_initial = True - - def perform(self, reevaluate=False): - if self.blackboard.shut_down_request: - if self.blackboard.current_state == RobotControlState.HARDWARE_PROBLEM: - self.blackboard.current_state = RobotControlState.SHUTDOWN - return "SHUTDOWN_WHILE_HARDWARE_PROBLEM" - else: - self.blackboard.current_state = RobotControlState.SHUTDOWN - return "SHUTDOWN_REQUESTED" - else: - if self.is_initial: - if not self.is_walkready(): - return "NOT_WALKREADY" - else: - self.is_initial = False - self.blackboard.current_state = RobotControlState.STARTUP - return "RUNNING" - - def is_walkready(self): - """ - We check if any joint is has an offset from the walkready pose which is higher than a threshold - """ - if self.blackboard.current_joint_state is None or self.blackboard.current_joint_state.name == []: - return False - i = 0 - for joint_name in self.blackboard.current_joint_state.name: - if joint_name == "HeadPan" or joint_name == "HeadTilt": - # we dont care about the head position - i += 1 - continue - if abs(math.degrees(self.blackboard.current_joint_state.position[i]) - - self.blackboard.walkready_pose_dict[joint_name]) > self.blackboard.walkready_pose_threshold: - return False - i += 1 - return True - - def get_reevaluate(self): - return True - - -class Stop(AbstractHCMDecisionElement): - """ - Handles manual stops - """ - - def perform(self, reevaluate=False): - if self.blackboard.stopped: - # we do an action sequence to go into stop and to stay there - return "STOPPED" - else: - return "FREE" - - def get_reevaluate(self): - return True - - -class Record(AbstractHCMDecisionElement): - """ - Decides if the robot is currently recording animations - """ - - def perform(self, reevaluate=False): - # check if the robot is currently recording animations - if self.blackboard.record_active: - self.blackboard.current_state = RobotControlState.RECORD - return "RECORD_ACTIVE" - else: - # robot is not recording - return "FREE" - - def get_reevaluate(self): - return True - - -class CheckMotors(AbstractHCMDecisionElement): - """ - Checks if we are getting information from the motors. - Since the HCM is not able to work without motor connection, we will stop if there are no values. - Needs to be checked before other sensors, since they also need the power to be able to response - """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.last_different_msg_time = Time(seconds=int(0), nanoseconds=0 % 1 * 1e9) - self.had_problem = False - - def perform(self, reevaluate=False): - self.clear_debug_data() - if self.blackboard.visualization_active: - # we will have no problems with hardware in visualization - return "OKAY" - - # we check if the values are actually changing, since the joint_state controller will publish the same message - # even if there is no connection anymore. But we don't want to go directly to hardware error if we just - # have a small break, since this can happen often due to loose cabling - if self.blackboard.previous_joint_state is not None and self.blackboard.current_joint_state is not None \ - and (self.blackboard.previous_joint_state.effort != self.blackboard.current_joint_state.effort - or self.blackboard.previous_joint_state.position != self.blackboard.current_joint_state.position) \ - and not self.blackboard.servo_diag_error: - self.last_different_msg_time = self.blackboard.current_time - - if self.blackboard.simulation_active: - # Some simulators will give exact same joint messages which look like errors, so ignore this case - if self.blackboard.last_motor_update_time != Time(seconds=int(0), nanoseconds=0 % 1 * 1e9): - return "OKAY" - else: - return "MOTORS_NOT_STARTED" - else: - if self.blackboard.servo_overload: - return "OVERLOAD" - - # check if we want to turn the motors off after not using them for a longer time - if self.blackboard.last_motor_goal_time is not None \ - and self.blackboard.current_time.nanoseconds / 1e9 - self.blackboard.last_motor_goal_time.nanoseconds / 1e9 \ - > self.blackboard.motor_off_time: - self.blackboard.node.get_logger().warn("Didn't recieve goals for " + str( - self.blackboard.motor_off_time) + " seconds. Will shut down the motors and wait for commands.", throttle_duration_sec=5) - self.publish_debug_data("Time since last motor goals", - self.blackboard.current_time.nanoseconds / 1e9 - self.blackboard.last_motor_goal_time.nanoseconds / 1e9) - self.blackboard.current_state = RobotControlState.MOTOR_OFF - # we do an action sequence to turn off the motors and stay in motor off - return "TURN_OFF" - - # see if we get no messages or always the exact same - if self.blackboard.current_time.nanoseconds / 1e9 - self.last_different_msg_time.nanoseconds / 1e9 > self.blackboard.motor_timeout_duration: - if self.blackboard.is_power_on: - if (self.blackboard.current_state == RobotControlState.STARTUP and - self.blackboard.current_time.nanoseconds / 1e9 - self.blackboard.start_time.nanoseconds / 1e9 < 10): - # we are still in startup phase, just wait and dont complain - return "MOTORS_NOT_STARTED" - else: - # tell that we have a hardware problem - self.had_problem = True - # wait for motors to connect - self.blackboard.current_state = RobotControlState.HARDWARE_PROBLEM - return "PROBLEM" - else: - # we have to turn the motors on - return "TURN_ON" - - if self.had_problem: - # had problem before, just tell that this is solved now - self.blackboard.node.get_logger().info("Motors are now connected. Will resume.") - self.had_problem = False - - # motors are on and we can continue - return "OKAY" - - def get_reevaluate(self): - return True - - -class CheckIMU(AbstractHCMDecisionElement): - """ - Checks if we are getting information from the IMU. - Since the HCM can not detect falls without it, we will shut everything down if we dont have an imu. - """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.last_msg = None - self.last_different_msg_time = Time(seconds=int(0), nanoseconds=0 % 1 * 1e9) - self.had_problem = False - - def perform(self, reevaluate=False): - if self.blackboard.visualization_active: - # In visualization, we do not have an IMU. Therefore, return OKAY to ignore that. - return "OKAY" - - # we will get always the same message if there is no connection, so check if it differs - if self.last_msg is not None and self.blackboard.imu_msg is not None \ - and not self.last_msg.orientation == self.blackboard.imu_msg.orientation \ - and not self.blackboard.imu_diag_error: - self.last_different_msg_time = self.blackboard.current_time - self.last_msg = self.blackboard.imu_msg - - if self.blackboard.simulation_active: - # Some simulators will give exact same IMU messages which look like errors, so ignore this case - if self.last_msg: - return "OKAY" - else: - return "IMU_NOT_STARTED" - - if self.blackboard.current_time.nanoseconds / 1e9 - self.last_different_msg_time.nanoseconds / 1e9 > self.blackboard.imu_timeout_duration: - if self.blackboard.current_state == RobotControlState.STARTUP and self.blackboard.current_time.nanoseconds / 1e9 - \ - self.blackboard.start_time.nanoseconds / 1e9 < 10: - # wait for the IMU to start - return "IMU_NOT_STARTED" - else: - self.blackboard.current_state = RobotControlState.HARDWARE_PROBLEM - self.had_problem = True - return "PROBLEM" - - if self.had_problem: - # had problem before, just tell that this is solved now - self.blackboard.node.get_logger().info("IMU is now connected. Will resume.") - self.had_problem = False - - return "OKAY" - - def get_reevaluate(self): - return True - - -class CheckPressureSensor(AbstractHCMDecisionElement): - """ - Checks connection to pressure sensors. - """ - - def __init__(self, blackboard, dsd, parameters=None): - super().__init__(blackboard, dsd, parameters) - self.last_pressure_values = None - self.last_different_msg_time = Time(seconds=int(0), nanoseconds=0 % 1 * 1e9) - self.had_problem = False - - def perform(self, reevaluate=False): - if self.blackboard.visualization_active: - # no pressure sensors is visualization, but thats okay - return "OKAY" - - if not self.blackboard.pressure_sensors_installed: - # no pressure sensors installed, no check necessary - return "OKAY" - if not self.blackboard.pressure_diag_error: - self.last_different_msg_time = self.blackboard.current_time - - if self.blackboard.current_time.nanoseconds / 1e9 - self.last_different_msg_time.nanoseconds / 1e9 > 0.1: - if self.blackboard.current_state == RobotControlState.STARTUP and self.blackboard.current_time.nanoseconds / 1e9 - \ - self.blackboard.start_time.nanoseconds / 1e9 < 10: - # wait for the pressure sensors to start - self.blackboard.current_state = RobotControlState.STARTUP - return "PRESSURE_NOT_STARTED" - else: - self.blackboard.current_state = RobotControlState.HARDWARE_PROBLEM - return "PROBLEM" - - if self.had_problem: - # had problem before, just tell that this is solved now - self.blackboard.node.get_logger().info("Pressure sensors are now connected. Will resume.") - self.had_problem = False - - return "OKAY" - - def get_reevaluate(self): - return True - - -class PickedUp(AbstractHCMDecisionElement): - """ - Decides if the robot is currently picked up - """ - - def perform(self, reevaluate=False): - if self.blackboard.visualization_active: - return "ON_GROUND" - # check if the robot is currently being picked up. foot have no connection to the ground, - # but robot is more or less upright (to differentiate from falling) - if self.blackboard.pressure_sensors_installed and not self.blackboard.simulation_active and \ - sum(self.blackboard.pressures) < 10 and \ - abs(self.blackboard.smooth_accel[0]) < self.blackboard.pickup_accel_threshold and \ - abs(self.blackboard.smooth_accel[1]) < self.blackboard.pickup_accel_threshold: - self.blackboard.current_state = RobotControlState.PICKED_UP - if not reevaluate: - speak("Picked up!", self.blackboard.speak_publisher, priority=50) - # we do an action sequence to go to walkready and stay in picked up state - return "PICKED_UP" - - # robot is not picked up - return "ON_GROUND" - - def get_reevaluate(self): - return True - - -class Falling(AbstractHCMDecisionElement): - """ - Decides if the robot is currently falling and has to act on this - """ - - def perform(self, reevaluate=False): - # check if the robot is currently falling - falling_direction = self.blackboard.fall_checker.check_falling(self.blackboard.gyro, self.blackboard.quaternion) - if self.blackboard.falling_detection_active and falling_direction is not None: - self.blackboard.current_state = RobotControlState.FALLING - - if falling_direction == self.blackboard.fall_checker.FRONT: - return "FALLING_FRONT" - if falling_direction == self.blackboard.fall_checker.BACK: - return "FALLING_BACK" - if falling_direction == self.blackboard.fall_checker.LEFT: - return "FALLING_LEFT" - if falling_direction == self.blackboard.fall_checker.RIGHT: - return "FALLING_RIGHT" - # robot is not fallen - return "NOT_FALLING" - - def get_reevaluate(self): - return True - - -class FallingClassifier(AbstractHCMDecisionElement): - - def perform(self, reevaluate=False): - if self.blackboard.falling_detection_active: - prediction = self.blackboard.classifier.smooth_classify(self.blackboard.imu_msg, - self.blackboard.current_joint_state, - self.blackboard.cop_l_msg, self.blackboard.cop_r_msg) - if prediction == 0: - return "NOT_FALLING" - else: - if not reevaluate: - self.blackboard.current_state = RobotControlState.FALLING - if prediction == 1: - return "FALLING_FRONT" - elif prediction == 2: - return "FALLING_BACK" - elif prediction == 3: - return "FALLING_LEFT" - elif prediction == 4: - return "FALLING_RIGHT" - else: - return "NOT_FALLING" - - def get_reevaluate(self): - return True - - -class Sitting(AbstractHCMDecisionElement): - """ - Decides if the robot is sitting (due to sitting down earlier). - """ - - def perform(self, reevaluate=False): - if self.blackboard.current_joint_state is None: - return "NO" - # simple check is looking at knee joint positions - # todo can be done more sophisticated - if self.blackboard.current_joint_state is None: - return "NO" - left_knee = 0 - right_knee = 0 - i = 0 - for joint_name in self.blackboard.current_joint_state.name: - if joint_name == "LKnee": - left_knee = self.blackboard.current_joint_state.position[i] - elif joint_name == "RKnee": - right_knee = self.blackboard.current_joint_state.position[i] - i += 1 - - if abs(left_knee) > 2.5 and abs(right_knee) > 2.5: - return "YES" - else: - return "NO" - - def get_reevaluate(self): - # we never have to reevaluate since this state of this can only be changed by decisions above it - return False - - -class Fallen(AbstractHCMDecisionElement): - """ - Decides if the robot is fallen and lying on the ground - """ - - def perform(self, reevaluate=False): - # check if the robot is currently laying on the ground - fallen_side = self.blackboard.fall_checker.check_fallen(self.blackboard.quaternion, self.blackboard.gyro) - if self.blackboard.is_stand_up_active and fallen_side is not None: - if not reevaluate: - self.blackboard.current_state = RobotControlState.FALLEN - # we play a stand up animation - if fallen_side == self.blackboard.fall_checker.FRONT: - return "FALLEN_FRONT" - if fallen_side == self.blackboard.fall_checker.BACK: - return "FALLEN_BACK" - if fallen_side == self.blackboard.fall_checker.RIGHT: - return "FALLEN_RIGHT" - if fallen_side == self.blackboard.fall_checker.LEFT: - return "FALLEN_LEFT" - else: - # robot is not fallen - return "NOT_FALLEN" - - def get_reevaluate(self): - return True - - -class ExternalAnimation(AbstractHCMDecisionElement): - """ - Decides if the robot is currently wants to play an animation comming from the behavior - """ - - def perform(self, reevaluate=False): - if self.blackboard.external_animation_running: - self.blackboard.current_state = RobotControlState.ANIMATION_RUNNING - return "ANIMATION_RUNNING" - else: - return "FREE" - - def get_reevaluate(self): - return True - - -class Walking(AbstractHCMDecisionElement): - """ - Decides if the robot is currently walking - """ - - def perform(self, reevaluate=False): - if self.blackboard.current_time.nanoseconds / 1e9 - self.blackboard.last_walking_goal_time.nanoseconds / 1e9 < 0.1: - self.blackboard.current_state = RobotControlState.WALKING - if self.blackboard.animation_requested: - self.blackboard.animation_requested = False - # we are walking but we have to stop to play an animation - return "STOP_WALKING" - else: - # we are walking and can stay like this - return "STAY_WALKING" - else: - return "NOT_WALKING" - - def get_reevaluate(self): - return True - - -class Kicking(AbstractHCMDecisionElement): - """ - Decides if the robot is currently kicking - """ - - def perform(self, reevaluate=False): - if self.blackboard.last_kick_feedback is not None and \ - (self.blackboard.node.get_clock().now() - self.blackboard.last_kick_feedback) < Duration(seconds=1): - self.blackboard.current_state = RobotControlState.KICKING - return 'KICKING' - else: - return 'NOT_KICKING' - - def get_reevaluate(self): - return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py new file mode 100644 index 000000000..e232a715d --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py @@ -0,0 +1,56 @@ +import math + +import numpy as np +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement +from bitbots_utils.transforms import quat2fused + +from bitbots_msgs.msg import RobotControlState + + +class Fallen(AbstractHCMDecisionElement): + """ + Decides if the robot is fallen and lying on the ground + """ + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + + # Get parameters + self.fallen_orientation_thresh = math.radians(self.blackboard.node.get_parameter("fallen_orientation_thresh").value) + self.fallen_angular_velocity_thresh = self.blackboard.node.get_parameter("fallen_angular_velocity_thresh").value + + def perform(self, reevaluate=False): + # Check of the fallen detection is active + if not self.blackboard.is_stand_up_active: + return "NOT_FALLEN" + + # Get angular velocity from the IMU + angular_velocity = self.blackboard.gyro + + # Check if the robot is rotating + if np.mean(np.abs(angular_velocity)) >= 0.2: + return "NOT_FALLEN" + + # Convert quaternion to fused angles + fused_roll, fused_pitch, _, _ = quat2fused(self.blackboard.quaternion, order="xyzw") + + # Decides which side is facing downwards. + if fused_pitch > self.fallen_orientation_thresh: + self.blackboard.node.get_logger().info("FALLEN TO THE FRONT") + return "FALLEN_FRONT" + + if fused_pitch < -self.fallen_orientation_thresh: + self.blackboard.node.get_logger().info("FALLEN TO THE BACK") + return "FALLEN_BACK" + + if fused_roll > self.fallen_orientation_thresh: + self.blackboard.node.get_logger().info("FALLEN TO THE RIGHT") + return "FALLEN_RIGHT" + + if fused_roll < -self.fallen_orientation_thresh: + self.blackboard.node.get_logger().info("FALLEN TO THE LEFT") + return "FALLEN_LEFT" + + return "NOT_FALLEN" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py new file mode 100644 index 000000000..2f46df28c --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py @@ -0,0 +1,131 @@ +import math +from enum import Enum + +import numpy as np +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement +from bitbots_utils.transforms import quat2fused +from rclpy.duration import Duration + + +class FallDirection(Enum): + STABLE = 0 + FRONT = 1 + BACK = 2 + LEFT = 3 + RIGHT = 4 + + +class Falling(AbstractHCMDecisionElement): + """ + Decides if the robot is currently falling and has to act on this + """ + + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + + # Get parameters + self.thresh_gyro_pitch = self.blackboard.node.get_parameter("falling_thresh_gyro_pitch").value + self.thresh_gyro_roll = self.blackboard.node.get_parameter("falling_thresh_gyro_roll").value + self.thresh_orient_pitch = math.radians(self.blackboard.node.get_parameter("falling_thresh_orient_pitch").value) + self.thresh_orient_roll = math.radians(self.blackboard.node.get_parameter("falling_thresh_orient_roll").value) + self.smoothing = self.blackboard.node.get_parameter("smooth_threshold").value + + # Initialize smoothing list that stores the last results + self.smoothing_list: list[FallDirection] = [] + + + + def perform(self, reevaluate=False): + """Checks if the robot is currently falling and in which direction.""" + # Check if detection is active + if not self.blackboard.falling_detection_active: + return "NOT_FALLING" + + # Get angular from the IMU + angular_velocity = self.blackboard.gyro + + # Convert orientation to fused angles + fused_roll, fused_pitch, _, _ = quat2fused(self.blackboard.quaternion, order="xyzw") + + # setting the fall quantification function + roll_fall_quantification = self.calc_fall_quantification( + self.thresh_orient_roll, + self.thresh_gyro_roll, + fused_roll, + angular_velocity[0]) + + pitch_fall_quantification = self.calc_fall_quantification( + self.thresh_orient_pitch, + self.thresh_gyro_pitch, + fused_pitch, + angular_velocity[1]) + + if roll_fall_quantification + pitch_fall_quantification == 0: + result = FallDirection.STABLE + else: + # compare quantification functions + if pitch_fall_quantification > roll_fall_quantification: + # detect the falling direction + if fused_pitch < 0: + result = FallDirection.BACK + # detect the falling direction + else: + result = FallDirection.FRONT + else: + # detect the falling direction + if fused_roll < 0: + result = FallDirection.LEFT + # detect the falling direction + else: + result = FallDirection.RIGHT + + # Prune old elements from smoothing history + self.smoothing_list = list(filter( + lambda x: x[0] > self.blackboard.node.get_clock().now() - Duration(seconds=self.smoothing), + self.smoothing_list)) + + # Add the current element + self.smoothing_list.append((self.blackboard.node.get_clock().now(), result)) + + # List only including the results not the whole tuples + results_list = list(zip(*self.smoothing_list))[1] + + # Check if stable is not in the list otherwise say we are stable + # This smooths the output but prevents the output of stable when jittering between e.g. right and front + if FallDirection.STABLE in results_list: + result = FallDirection.STABLE + + # Return the appropriate result + if result == FallDirection.STABLE: + return "NOT_FALLING" + elif result == FallDirection.FRONT: + return "FALLING_FRONT" + elif result == FallDirection.BACK: + return "FALLING_BACK" + elif result == FallDirection.LEFT: + return "FALLING_LEFT" + elif result == FallDirection.RIGHT: + return "FALLING_RIGHT" + else: + raise ValueError("Unknown falling direction") + + def calc_fall_quantification(self, falling_threshold_orientation, falling_threshold_gyro, current_axis_euler, + current_axis__gyro): + # check if you are moving forward or away from the perpendicular position, by comparing the signs. + moving_more_upright = np.sign(current_axis_euler) != np.sign(current_axis__gyro) + + # Check if the orientation is over the point of no return threshold + over_point_of_no_return = abs(current_axis_euler) > falling_threshold_orientation + + # Calculate quantification if we are moving away from our upright position or if we are over the point of no return + if not moving_more_upright or over_point_of_no_return: + # calculatiung the orentation skalar for the threshold + skalar = max((falling_threshold_orientation - abs(current_axis_euler)) / falling_threshold_orientation, 0) + # checking if the rotation velocity is lower than the the threshold + if falling_threshold_gyro * skalar < abs(current_axis__gyro): + # returning the fall quantification function + return abs(current_axis__gyro) * (1 - skalar) + return 0 + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/kicking.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/kicking.py new file mode 100644 index 000000000..b1000255a --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/kicking.py @@ -0,0 +1,28 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement + + +class RecentKickGoals(AbstractHCMDecisionElement): + """ + Decides if the kick is currently sending joint commands + """ + + def perform(self, reevaluate=False): + # Check if we have received a kick goal at all + if self.blackboard.last_kick_goal_time is None: + return "NOT_KICKING" + + # Calculate the time delta between now and the last kick goal + time_delta = self.blackboard.node.get_clock().now().nanoseconds / 1e9 - self.blackboard.last_kick_goal_time.nanoseconds / 1e9 + + # Log the time delta between now and the last kick goal + self.publish_debug_data("Last Kick Goal Time Delta", time_delta) + + # If the time delta is smaller enough, we are still kicking + if time_delta < 0.1: + # we are kicking and can stay like this + return "KICKING" + else: + return "NOT_KICKING" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/pickup.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/pickup.py new file mode 100644 index 000000000..8d3d690a1 --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/pickup.py @@ -0,0 +1,28 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement +from humanoid_league_speaker.speaker import speak + + +class PickedUp(AbstractHCMDecisionElement): + """ + Decides if the robot is currently picked up + """ + + def perform(self, reevaluate=False): + if self.blackboard.visualization_active: + return "ON_GROUND" + # check if the robot is currently being picked up. foot have no connection to the ground, + # but robot is more or less upright (to differentiate from falling) + if self.blackboard.pressure_sensors_installed and not self.blackboard.simulation_active and \ + sum(self.blackboard.pressures) < 10 and \ + abs(self.blackboard.smooth_accel[0]) < self.blackboard.pickup_accel_threshold and \ + abs(self.blackboard.smooth_accel[1]) < self.blackboard.pickup_accel_threshold: + if not reevaluate: + speak("Picked up!", self.blackboard.speak_publisher, priority=50) + # we do an action sequence to go to walkready and stay in picked up state + return "PICKED_UP" + + # robot is not picked up + return "ON_GROUND" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/startup.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/startup.py new file mode 100644 index 000000000..51f3de903 --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/startup.py @@ -0,0 +1,19 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement + + +class StartHCM(AbstractHCMDecisionElement): + """ + Initializes HCM. + """ + def __init__(self, blackboard, dsd, parameters=None): + super().__init__(blackboard, dsd, parameters) + self.is_initial = True + + def perform(self, reevaluate=False): + if self.is_initial: + self.is_initial = False + return "START_UP" + return "RUNNING" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/stop.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/stop.py new file mode 100644 index 000000000..86032413c --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/stop.py @@ -0,0 +1,16 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement + +class Stop(AbstractHCMDecisionElement): + """ + Handles manual stops + """ + + def perform(self, reevaluate=False): + if self.blackboard.stopped: + # we do an action sequence to go into stop and to stay there + return "STOPPED" + else: + return "FREE" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/walking.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/walking.py new file mode 100644 index 000000000..d406e3a04 --- /dev/null +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/walking.py @@ -0,0 +1,28 @@ +from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement + + +class RecentWalkingGoals(AbstractHCMDecisionElement): + """ + Decides if the robot is currently getting joint commands to from the walking node + """ + + def perform(self, reevaluate=False): + # Check if we have received a walking goal at all + if self.blackboard.last_walking_goal_time is None: + return "NOT_WALKING" + + # Calculate the time delta between now and the last walking goal + time_delta = self.blackboard.node.get_clock().now().nanoseconds / 1e9 - self.blackboard.last_walking_goal_time.nanoseconds / 1e9 + + # Log the time delta between now and the last walking goal + self.publish_debug_data("Last Walking Goal Time Delta", time_delta) + + # If the time delta is smaller enough, we are still walking + if time_delta < 0.1: + # we are walking and can stay like this + return "STAY_WALKING" + else: + return "NOT_WALKING" + + def get_reevaluate(self): + return True diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd b/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd index a2bfc5941..f6ea5b920 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd @@ -1,48 +1,37 @@ -#ShutDownProcedure -$PickedUp - PICKED_UP --> @TurnMotorsOff, @StayShutDown - ON_GROUND --> @PlayAnimationSitDown, @TurnMotorsOff, @StayShutDown - -->HCM $StartHCM - NOT_WALKREADY --> @Wait + time:0.1 + r:false, @PlayAnimationDynup + direction:walkready + initial:true + r:false - SHUTDOWN_REQUESTED --> #ShutDownProcedure - SHUTDOWN_WHILE_HARDWARE_PROBLEM --> @StayShutDown + START_UP --> @RobotStateStartup, @Wait + time:0.1 + r:false, @PlayAnimationDynup + direction:walkready, @Wait RUNNING --> $Stop - STOPPED --> @ForceStopWalking, @PlayAnimationDynup + direction:walkready + initial:true + r:false, @StayStopped - FREE -->$Record - RECORD_ACTIVE --> @StayRecord + STOPPED --> @CancelGoals, @StopWalking, @PlayAnimationDynup + direction:walkready, @Wait + FREE -->$RecordAnimation + RECORD_ACTIVE --> @RobotStateRecord, @Wait FREE --> $CheckMotors - MOTORS_NOT_STARTED --> @WaitForMotorStartup - OVERLOAD --> @PlayAnimationDynup + direction:descend, @TurnMotorsOff, @StayMotorsOff - PROBLEM --> @WaitForMotors - TURN_OFF --> @PlayAnimationDynup + direction:descend, @TurnMotorsOff, @StayMotorsOff - TURN_ON --> @TurnMotorsOn, @PlayAnimationDynup + direction:rise + MOTORS_NOT_STARTED --> @RobotStateStartup, @WaitForMotorStartup + OVERLOAD --> @RobotStateMotorOff, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @TurnMotorsOff, @Wait + PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors + TURN_ON --> @TurnMotorsOn, @PlayAnimationDynup + direction:walkready, @Wait OKAY --> $CheckIMU - IMU_NOT_STARTED --> @WaitForIMUStartup - PROBLEM --> @WaitForIMU + IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup + PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU OKAY --> $CheckPressureSensor - PRESSURE_NOT_STARTED --> @WaitForPressureStartup - PROBLEM --> @WaitForPressure + PRESSURE_NOT_STARTED --> @RobotStateStartup, @WaitForPressureStartup + PROBLEM --> @RobotStateHardwareProblem, @WaitForPressure OKAY --> $PickedUp - PICKED_UP --> @PlayAnimationDynup + direction:walkready, @StayPickedUp + PICKED_UP --> @RobotStatePickedUp, @PlayAnimationDynup + direction:walkready, @Wait ON_GROUND --> $Fallen - FALLEN_FRONT --> @CancelGoals, @PlayAnimationDynup + direction:front - FALLEN_BACK --> @CancelGoals, @SetFootZero, @PlayAnimationDynup + direction:back - FALLEN_RIGHT --> @CancelGoals, @TurnRightToBack + duration:1.5, @PlayAnimationDynup + direction:back - FALLEN_LEFT --> @CancelGoals, @TurnLeftToBack + duration:1.5, @PlayAnimationDynup + direction:back + FALLEN_FRONT --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationDynup + direction:front + FALLEN_BACK --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationDynup + direction:back + FALLEN_RIGHT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningBackRight + FALLEN_LEFT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningBackLeft NOT_FALLEN --> $Falling - FALLING_LEFT --> @CancelGoals, @FallingPoseLeft + duration:0.5 - FALLING_RIGHT --> @CancelGoals, @FallingPoseRight + duration:0.5 - FALLING_FRONT --> @CancelGoals, @FallingPoseFront + duration:0.5 - FALLING_BACK --> @CancelGoals, @FallingPoseBack + duration:0.5 - NOT_FALLING --> $Sitting - YES --> @PlayAnimationDynup + direction:rise - NO --> $ExternalAnimation - ANIMATION_RUNNING --> @StayAnimationRunning - FREE --> $Walking - STOP_WALKING --> @StopWalking - STAY_WALKING --> @StayWalking - NOT_WALKING --> $Kicking - KICKING --> @StayKicking - NOT_KICKING --> @StayControlable + FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait + FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait + FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait + FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait + NOT_FALLING --> $PlayingExternalAnimation + ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait + FREE --> $RecentWalkingGoals + STAY_WALKING --> @RobotStateWalking, @Wait + NOT_WALKING --> $RecentKickGoals + KICKING --> @RobotStateKicking, @Wait + NOT_KICKING --> @RobotStateControllable, @Wait diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py b/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py index 755a41fbb..f79154a04 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py @@ -1,145 +1,114 @@ -import json -import os from typing import List, Optional import numpy -from ament_index_python import get_package_share_directory -from bitbots_hcm.fall_checker import FallChecker -from bitbots_hcm.fall_classifier import FallClassifier -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from geometry_msgs.msg import PointStamped, Twist +from geometry_msgs.msg import Twist from rclpy.action import ActionClient from rclpy.node import Node +from rclpy.task import Future from rclpy.time import Time from sensor_msgs.msg import Imu, JointState from std_msgs.msg import Empty as EmptyMsg from std_srvs.srv import Empty as EmptySrv from std_srvs.srv import SetBool -from bitbots_msgs.action import Dynup -from bitbots_msgs.msg import JointCommand -from humanoid_league_msgs.action import PlayAnimation -from humanoid_league_msgs.msg import RobotControlState +from bitbots_msgs.action import Dynup, PlayAnimation +from bitbots_msgs.msg import Audio, RobotControlState class HcmBlackboard(): def __init__(self, node: Node): self.node = node + + # Basic state self.current_state: RobotControlState = RobotControlState.STARTUP self.stopped: bool = False - self.shut_down_request: bool = False - self.simulation_active = self.node.get_parameter('simulation_active').value - self.visualization_active = self.node.get_parameter('visualization_active').value - - # this is used to prevent calling Time a lot, which takes some time - # we assume that the time does not change during one update cycle - self.current_time = self.node.get_clock().now() - self.start_time = self.current_time - # Imu + + # Save start time + self.start_time: Time = self.node.get_clock().now() + + # Get parameters + self.simulation_active: bool = self.node.get_parameter('simulation_active').value + self.visualization_active: bool = self.node.get_parameter('visualization_active').value + self.pickup_accel_threshold: float = self.node.get_parameter('pick_up_accel_threshold').value + self.pressure_sensors_installed: bool = self.node.get_parameter('pressure_sensors_installed').value + + # Create services + self.foot_zero_service = self.node.create_client(EmptySrv, "set_foot_zero") + self.motor_switch_service = self.node.create_client(SetBool, 'core/switch_power') + + # Create action clients and corresponding goal handles + self.animation_action_client: ActionClient = ActionClient(self.node, PlayAnimation, 'animation') + self.animation_action_current_goal: Optional[Future] = None + self.dynup_action_client: ActionClient = ActionClient(self.node, Dynup, 'dynup') + self.dynup_action_current_goal: Optional[Future] = None + + # Create publishers + self.walk_pub = self.node.create_publisher(Twist, "cmd_vel", 1) + self.cancel_path_planning_pub = self.node.create_publisher(EmptyMsg, "pathfinding/cancel", 1) + self.speak_publisher = self.node.create_publisher(Audio, 'speak', 1) + + # Latest imu data self.accel = numpy.array([0, 0, 0]) self.gyro = numpy.array([0, 0, 0]) self.smooth_accel = numpy.array([0, 0, 0]) self.smooth_gyro = numpy.array([0, 0, 0]) self.not_much_smoothed_gyro = numpy.array([0, 0, 0]) - self.quaternion = numpy.array([0, 0, 0, 0.21]) - self.pickup_accel_threshold: float = 1000.0 - - # Pressure sensors - self.pressure_sensors_installed = self.node.get_parameter('pressure_sensors_installed').value - # initialize values high to prevent wrongly thinking the robot is picked up during start or in simulation - self.pressures: List[float] = [100.0] * 8 - foot_zero_service_name = self.node.get_parameter("foot_zero_service").value - self.foot_zero_service = self.node.create_client(EmptySrv, foot_zero_service_name) - - self.motor_switch_service = self.node.create_client(SetBool, 'core/switch_power') + self.quaternion = numpy.array([0, 0, 0, 1.0]) # Animation - self.animation_action_client: Optional[ActionClient] = None - self.animation_action_current_goal: Optional[PlayAnimation.Goal] = None - self.dynup_action_client: Optional[ActionClient] = None - self.dynup_action_current_goal: Optional[Dynup.Goal] = None - self.last_animation_goal_time = self.node.get_clock().now() - self.external_animation_running: bool = False + # Animation states self.animation_requested: bool = False - if self.simulation_active: - self.walkready_animation = self.node.get_parameter("animations.walkready_sim").value - else: - self.walkready_animation = self.node.get_parameter("animations.walkready").value - self.falling_animation_front = self.node.get_parameter("animations.falling_front").value - self.falling_animation_back = self.node.get_parameter("animations.falling_back").value - self.falling_animation_left = self.node.get_parameter("animations.falling_left").value - self.falling_animation_right = self.node.get_parameter("animations.falling_right").value - self.stop_animation = self.node.get_parameter("animations.penalty").value - self.sit_down_animation = self.node.get_parameter("animations.sit_down").value - self.motor_off_animation = self.node.get_parameter("animations.motor_off").value - self.stand_up_front_animation = self.node.get_parameter("animations.stand_up_front").value - self.stand_up_back_animation = self.node.get_parameter("animations.stand_up_back").value - self.stand_up_left_animation = self.node.get_parameter("animations.stand_up_left").value - self.stand_up_right_animation = self.node.get_parameter("animations.stand_up_right").value - # motors - # initialize with current time, or motors will be turned off on start - self.last_motor_goal_time = self.node.get_clock().now() - self.last_motor_update_time = Time() - self.motor_timeout_duration = self.node.get_parameter("motor_timeout_duration").value - self.motor_off_time = self.node.get_parameter("motor_off_time").value - self.imu_timeout_duration = self.node.get_parameter("imu_timeout_duration").value + self.external_animation_running: bool = False + self.last_animation_goal_time: Time = self.node.get_clock().now() + self.record_active: bool = False + + # Get animation parameters + self.animation_name_falling_front: str = self.node.get_parameter("animations.falling_front").value + self.animation_name_falling_back: str = self.node.get_parameter("animations.falling_back").value + self.animation_name_falling_left: str = self.node.get_parameter("animations.falling_left").value + self.animation_name_falling_right: str = self.node.get_parameter("animations.falling_right").value + self.animation_name_turning_back_left: str = self.node.get_parameter("animations.turning_back_left").value + self.animation_name_turning_back_right: str = self.node.get_parameter("animations.turning_back_right").value + + # Motor State self.current_joint_state: Optional[JointState] = None self.previous_joint_state: Optional[JointState] = None - anim_package = self.node.get_parameter("animations.anim_package").value - path = get_package_share_directory(anim_package) - path = os.path.join(path, 'animations/motion/', self.walkready_animation + '.json') - with open(path, 'r') as f: - json_data = json.load(f) - keyframes: List[dict] = json_data["keyframes"] - self.walkready_pose_dict: dict = keyframes[-1]["goals"] - self.walkready_pose_threshold = self.node.get_parameter("animations.walkready_pose_threshold").value + self.last_different_joint_state_time: Optional[Time] = None self.is_power_on: bool = False - # walking - self.last_walking_goal_time = self.node.get_clock().now() - self.walk_pub = self.node.create_publisher(Twist, "cmd_vel", 1) - self.record_active: bool = False + # Motor Parameters + self.motor_timeout_duration: float = self.node.get_parameter("motor_timeout_duration").value + self.motor_off_time: float = self.node.get_parameter("motor_off_time").value + self.imu_timeout_duration: float = self.node.get_parameter("imu_timeout_duration").value + + # Walking state + self.last_walking_goal_time: Optional[Time] = None - # falling - self.fall_checker = FallChecker(self.node) + # Falling + # Paramerters self.is_stand_up_active = self.node.get_parameter('stand_up_active').value self.falling_detection_active = self.node.get_parameter('falling_active').value - self.joint_pub = self.node.create_publisher(JointCommand, "DynamixelController/command", 1) - # kicking - self.last_kick_feedback: Optional[Time] = None + # Kicking + # State + self.last_kick_goal_time: Optional[Time] = None - # direct messages for falling classier - # todo needs refactoring - path = get_package_share_directory('bitbots_hcm') - smooth_threshold = self.node.get_parameter('smooth_threshold').value - self.classifier = FallClassifier(path + "/classifier/", smooth_threshold=smooth_threshold) + # IMU state self.imu_msg: Optional[Imu] = None - self.cop_l_msg: Optional[PointStamped] = None - self.cop_r_msg: Optional[PointStamped] = None + self.previous_imu_msg: Optional[Imu] = None + self.last_different_imu_state_time: Optional[Time] = None + + # Pressure sensors + # Initialize values high to prevent wrongly thinking the robot is picked up during start or in simulation + self.pressures: List[float] = [100.0] * 8 + self.previous_pressures: List[float] = self.pressures.copy() + self.last_different_pressure_state_time: Optional[Time] = None + # Diagnostics state self.servo_diag_error: bool = False self.servo_overload: bool = False self.imu_diag_error: bool = False self.pressure_diag_error: bool = False - self.move_base_cancel_pub = self.node.create_publisher(EmptyMsg, "pathfinding/cancel", 1) - - def diag_cb(self, msg: DiagnosticArray): - status: DiagnosticStatus - for status in msg.status: - if "//Servos/" in status.name: - if status.level == DiagnosticStatus.ERROR and "Overload" in status.message: - self.servo_overload = True - elif "//Servos" in status.name: - self.servo_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE) - elif "//IMU" in status.name: - self.imu_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE) - elif "//Pressure" in status.name: - self.pressure_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE) - - def last_kick_feedback_callback(self, msg: PlayAnimation.Feedback): - self.last_kick_feedback = self.node.get_clock().now() - - def cancel_move_base_goal(self): - self.move_base_cancel_pub.publish(EmptyMsg()) + def cancel_path_planning(self): + self.cancel_path_planning_pub.publish(EmptyMsg()) diff --git a/bitbots_hcm/bitbots_hcm/humanoid_control_module.py b/bitbots_hcm/bitbots_hcm/humanoid_control_module.py index 1c9826357..d2184a359 100755 --- a/bitbots_hcm/bitbots_hcm/humanoid_control_module.py +++ b/bitbots_hcm/bitbots_hcm/humanoid_control_module.py @@ -1,203 +1,193 @@ #!/usr/bin/env python3 -import numpy + +import os +import threading + import rclpy +from ament_index_python import get_package_share_directory +from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_utils.utils import get_parameters_from_ros_yaml +from builtin_interfaces.msg import Time as TimeMsg +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from humanoid_league_speaker.speaker import speak +from rcl_interfaces.msg import Parameter as ParameterMsg from rclpy.duration import Duration -from rclpy.node import Node -from rclpy.action import ActionClient from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node from rclpy.parameter import Parameter +from rclpy.serialization import deserialize_message from rclpy.time import Time - -from geometry_msgs.msg import PointStamped - -from std_msgs.msg import Bool +from ros2_numpy import numpify from sensor_msgs.msg import Imu, JointState -from diagnostic_msgs.msg import DiagnosticArray - -from humanoid_league_msgs.msg import RobotControlState, Audio -from humanoid_league_msgs.action import PlayAnimation -from humanoid_league_speaker.speaker import speak -from bitbots_msgs.msg import FootPressure -from bitbots_msgs.action import Dynup +from std_msgs.msg import Bool -from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard +from bitbots_msgs.msg import FootPressure, RobotControlState from dynamic_stack_decider.dsd import DSD -import os -import threading -from bitbots_utils.utils import get_parameters_from_ros_yaml -from ament_index_python import get_package_share_directory -from rcl_interfaces.msg import Parameter as ParameterMsg -from rclpy.serialization import deserialize_message -from builtin_interfaces.msg import Time as TimeMsg -class HardwareControlManager: +class HardwareControlManager: def __init__(self, use_sim_time, simulation_active, visualization_active): rclpy.init(args=None) node_name = "hcm_py" + + # Load parameters from yaml file because this is a hacky cpp python hybrid node for performance reasons parameter_msgs: list(ParameterMsg) = get_parameters_from_ros_yaml( node_name, f"{get_package_share_directory('bitbots_hcm')}/config/hcm_wolfgang.yaml", use_wildcard=True) parameters = [] for parameter_msg in parameter_msgs: parameters.append(Parameter.from_parameter_msg(parameter_msg)) if use_sim_time: - parameters.append(Parameter("use_sime_time", type_=Parameter.Type.BOOL, value=True)) + parameters.append(Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=True)) if simulation_active: parameters.append(Parameter("simulation_active", type_=Parameter.Type.BOOL, value=True)) if visualization_active: parameters.append(Parameter("visualization_active", type_=Parameter.Type.BOOL, value=True)) + + # Create Python node self.node = Node(node_name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=parameters) - # create own executor for python part + + # Create own executor for Python part multi_executor = MultiThreadedExecutor() multi_executor.add_node(self.node) self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), 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_clock().sleep_for(Duration(seconds=0.5)) self.node.get_logger().debug("Starting hcm") - self.simulation_active = self.node.get_parameter("simulation_active") - # dsd + + # Create Dynamic Stack Decider Blackboard 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" + # Create Dynamic Stack Decider self.dsd = DSD(self.blackboard, "debug/dsd/hcm", node=self.node) + # Get the path to the python actions and decisions + dirname = os.path.join(get_package_share_directory('bitbots_hcm'), "hcm_dsd") + # Register actions and decisions self.dsd.register_actions(os.path.join(dirname, 'actions')) self.dsd.register_decisions(os.path.join(dirname, 'decisions')) + # Load the behavior file self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd')) + + # Flag to deactivate the HCM self.hcm_deactivated = False - # Publisher / subscriber - 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) + # Create subscribers self.node.create_subscription(Bool, "pause", self.pause, 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.node.create_subscription(DiagnosticArray, "diagnostics_agg", self.diag_cb, 1) + + # Store time of the last tick + self.last_tick_start_time = self.node.get_clock().now() - self.last_loop_start_time = self.node.get_clock().now() + # Anounce the HCM startup + speak("Starting HCM", self.blackboard.speak_publisher, priority=50) - def deactivate_cb(self, msg): + def tick(self): + """ + Keeps updating the DSD and publishes its current state. + All the forwarding of joint goals is directly done in the callbacks to reduce latency. + """ + # Store the time of the current tick + tick_start_time = self.node.get_clock().now() + # This can happen in simulation due to bad implementation in rclpy + if (self.last_tick_start_time != tick_start_time): + self.last_tick_start_time = tick_start_time + # Do not perform any behavior if the HCM is deactivated + if self.hcm_deactivated: + self.blackboard.current_state = RobotControlState.CONTROLLABLE + else: + try: + # Step the dsd + self.dsd.update() + except IndexError: + # this error will happen during shutdown procedure, just ignore it + pass + + def deactivate_cb(self, msg: Bool): + """ Deactivates the HCM. """ self.hcm_deactivated = msg.data - def pause(self, msg): + def pause(self, msg: Bool): """ Updates the stop state for the state machine""" self.blackboard.stopped = msg.data - 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 power_cb(self, msg): + def power_cb(self, msg: Bool): + """ Updates the power state. """ self.blackboard.is_power_on = msg.data - def get_state(self): + def diag_cb(self, msg: DiagnosticArray): + """ Updates the diagnostic state. """ + status: DiagnosticStatus + for status in msg.status: + if "//Servos/" in status.name: + if status.level == DiagnosticStatus.ERROR and "Overload" in status.message: + self.blackboard.servo_overload = True + elif "//Servos" in status.name: + self.blackboard.servo_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE) + elif "//IMU" in status.name: + self.blackboard.imu_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE) + elif "//Pressure" in status.name: + self.blackboard.pressure_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE) + + def get_state(self) -> RobotControlState: + """ Returns the current state of the HCM. """ return self.blackboard.current_state - def 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. """ - if self.blackboard.shut_down_request and not self.simulation_active: - self.on_shutdown_hook() - else: - loop_start_time = self.node.get_clock().now() - #can happen in simulation due to bad implementation in rclpy - if (self.last_loop_start_time != loop_start_time): - self.last_loop_start_time = loop_start_time - if self.hcm_deactivated: - self.blackboard.current_state = RobotControlState.CONTROLLABLE - else: - self.blackboard.current_time = self.node.get_clock().now() - try: - self.dsd.update() - except IndexError: - # this error will happen during shutdown procedure, just ignore it - pass - - 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 set_last_animation_goal_time(self, time_msg_serialized): - self.blackboard.last_animation_goal_time = deserialize_message(time_msg_serialized, TimeMsg) + # The following methods are used to set the blackboard values from the cpp part - def set_animation_requested(self, animation_requested): + def set_animation_requested(self, animation_requested: bool): self.blackboard.animation_requested = animation_requested - def set_external_animation_running(self, running): + def set_external_animation_running(self, running: bool): self.blackboard.external_animation_running = running - def set_record_active(self, active): + def set_record_active(self, active: bool): self.blackboard.record_active = active - def set_last_walking_goal_time(self, time_msg_serialized): + def set_last_animation_goal_time(self, time_msg_serialized: bytes): + self.blackboard.last_animation_goal_time = deserialize_message(time_msg_serialized, TimeMsg) + + def set_last_walking_goal_time(self, time_msg_serialized: bytes): self.blackboard.last_walking_goal_time = Time.from_msg(deserialize_message(time_msg_serialized, TimeMsg)) - def set_last_motor_update_time(self, time_msg_serialized): - self.blackboard.last_motor_update_time = deserialize_message(time_msg_serialized, TimeMsg) + def set_last_kick_goal_time(self, time_msg_serialized: bytes): + self.blackboard.last_kick_goal_time = Time.from_msg(deserialize_message(time_msg_serialized, TimeMsg)) - def set_current_joint_state(self, joint_state_msg_serialized): + def set_current_joint_state(self, joint_state_msg_serialized: bytes): self.blackboard.previous_joint_state = self.blackboard.current_joint_state self.blackboard.current_joint_state = deserialize_message(joint_state_msg_serialized, JointState) - def set_cop(self, cop_msg_serialized, left): - if left: - self.blackboard.cop_l_msg = deserialize_message(cop_msg_serialized, PointStamped) - else: - self.blackboard.cop_r_msg = deserialize_message(cop_msg_serialized, PointStamped) - - def set_pressure_left(self, pressure_msg_serialized): - msg = deserialize_message(pressure_msg_serialized, FootPressure) + def set_pressure_left(self, pressure_msg_serialized: bytes): + msg: FootPressure = deserialize_message(pressure_msg_serialized, FootPressure) + self.blackboard.previous_pressures = self.blackboard.pressures 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 set_pressure_right(self, pressure_msg_serialized): - msg = deserialize_message(pressure_msg_serialized, FootPressure) + def set_pressure_right(self, pressure_msg_serialized: bytes): + msg: FootPressure = deserialize_message(pressure_msg_serialized, FootPressure) + self.blackboard.previous_pressures = self.blackboard.pressures 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 set_imu(self, imu_msg_serialized): - msg = deserialize_message(imu_msg_serialized, Imu) - - 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) + def set_imu(self, imu_msg_serialized: bytes): + self.blackboard.previous_imu_msg = self.blackboard.imu_msg + + msg: Imu = deserialize_message(imu_msg_serialized, Imu) + + self.blackboard.accel = numpify(msg.linear_acceleration) + self.blackboard.gyro = numpify(msg.angular_velocity) + self.blackboard.quaternion = numpify(msg.orientation) + + self.blackboard.smooth_gyro = 0.95 * self.blackboard.smooth_gyro + 0.05 * self.blackboard.gyro + self.blackboard.smooth_accel = 0.99 * self.blackboard.smooth_accel + 0.01 * self.blackboard.accel + self.blackboard.not_much_smoothed_gyro = 0.5 * self.blackboard.not_much_smoothed_gyro + 0.5 * self.blackboard.gyro self.blackboard.imu_msg = msg diff --git a/bitbots_hcm/bitbots_hcm/training/dataset.py b/bitbots_hcm/bitbots_hcm/training/dataset.py deleted file mode 100644 index 57f1811aa..000000000 --- a/bitbots_hcm/bitbots_hcm/training/dataset.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python3 -import tf -from sensor_msgs.msg import JointState, Imu, Image -from geometry_msgs.msg import Point - -import math -import numpy as np -from bitbots_hcm.fall_classifier import get_data_from_msgs - - -class Dataset: - - def __init__(self): - self.frames = [] - self.fall_impact_time = None - self.fall_type = None - - def append_frame(self, time, imu, joint_states, cop_l, cop_r, image): - frame = Frame(time, imu, joint_states, cop_l, cop_r, image) - self.frames.append(frame) - - def as_scikit_data(self, imu_raw=True, imu_orient=True, joint_states=True, imu_fused=True, cop=True): - X = [] - y = [] - for frame in self.frames: - X.append( - frame.get_data(imu_raw=imu_raw, imu_orient=imu_orient, joint_states=joint_states, imu_fused=imu_fused, - cop=cop)) - y.append(frame.label) - return X, y - - def get_frame_index_by_time(self, time): - difference = np.inf - index = None - for i in range(0, len(self.frames)): - if abs(time - self.frames[i].time) < difference: - index = i - difference = abs(time - self.frames[i].time) - return index - - def set_all_labels_to(self, label): - if label == "stable": - int_label = 0 - elif label == "front": - int_label = 1 - elif label == "back": - int_label = 2 - elif label == "left": - int_label = 3 - elif label == "right": - int_label = 4 - else: - print("Wrong label") - exit(1) - - for frame in self.frames: - frame.label = int_label - - def append_dataset(self, dataset): - self.frames = self.frames + dataset.frames - - -class Frame: - - def __init__(self, time, joint_states, imu, cop_l, cop_r, image): - self.time = time - self.imu: Imu = imu - self.joint_states: JointState = joint_states - self.cop_l: Point = cop_l - self.cop_r: Point = cop_r - self.image: Image = image - self.label = -1 - - def get_label(self): - if self.label == -2: - return "removed" - elif self.label == -1: - return "unlabeled" - elif self.label == 0: - return "stable" - elif self.label == 1: - return "front" - elif self.label == 2: - return "back" - elif self.label == 3: - return "left" - elif self.label == 4: - return "right" - else: - print(F"Label number {self.label} not clear, will use unlabeled") - return "unlabeled" - - def get_data(self, imu_raw=True, imu_orient=True, joint_states=True, imu_fused=True, cop=True): - return get_data_from_msgs(self.imu, self.joint_states, self.cop_l, self.cop_r, imu_raw, imu_orient, - joint_states, imu_fused, cop) diff --git a/bitbots_hcm/bitbots_hcm/training/label.py b/bitbots_hcm/bitbots_hcm/training/label.py deleted file mode 100644 index d0a63f01d..000000000 --- a/bitbots_hcm/bitbots_hcm/training/label.py +++ /dev/null @@ -1,132 +0,0 @@ -import pickle -import random -import rclpy -from rclpy.node import Node -import keyboard - -from geometry_msgs.msg import PointStamped -from sensor_msgs.msg import Imu, JointState - -from bitbots_hcm.training.dataset import Dataset, Frame - -imu = None -joint_states = None -cop_left = None -cop_right = None - -rclpy.init(args=None) - - -def imu_cb(msg): - global imu - imu = msg - - -def joint_state_cb(msg): - global joint_states - joint_states = msg - - -def cop_left_cb(msg): - global cop_left - cop_left = msg - - -def cop_right_cb(msg): - global cop_right - cop_right = msg - - -dataset = Dataset() - -rospy.Subscriber("/imu/data", Imu, imu_cb, tcp_nodelay=True) -rospy.Subscriber("/joint_states", JointState, joint_state_cb, tcp_nodelay=True) -rospy.Subscriber("/cop_l", PointStamped, cop_left_cb, tcp_nodelay=True) -rospy.Subscriber("/cop_r", PointStamped, cop_right_cb, tcp_nodelay=True) - - -# set keys -def key_cb(key): - global current_key - current_key = key - - -current_key = '5' -previous_key = None -keyboard.add_hotkey('5', key_cb, args='5') -keyboard.add_hotkey('0', key_cb, args='0') -keyboard.add_hotkey('1', key_cb, args='1') -keyboard.add_hotkey('2', key_cb, args='2') -keyboard.add_hotkey('3', key_cb, args='3') -keyboard.add_hotkey('4', key_cb, args='4') -keyboard.add_hotkey('s', key_cb, args='s') - -frequency = 200 -rate = self.create_rate(frequency) -print("Press \'s\' top stop. 0,1,2,3,4 to label stable,front,back,left,right") -while True: - frame = Frame(self.get_clock().now(), joint_states=joint_states, imu=imu, cop_l=cop_left, cop_r=cop_right, image=None) - if current_key == '5': - if previous_key != '5': - print("Not labeling") - previous_key = current_key - continue - elif current_key == '0': - if previous_key != '0': - print("Stable") - previous_key = current_key - frame.label = 0 - elif current_key == '1': - if previous_key != '1': - print("Front") - previous_key = current_key - frame.label = 1 - elif current_key == '2': - if previous_key != '2': - print("Back") - previous_key = current_key - frame.label = 2 - elif current_key == '3': - if previous_key != '3': - print("Left") - previous_key = current_key - frame.label = 3 - elif current_key == '4': - if previous_key != '4': - print("Right") - previous_key = current_key - frame.label = 4 - elif current_key == 's': - break - else: - print("key not known") - continue - dataset.frames.append(frame) - rate.sleep() - -print(F"Recorded {len(dataset.frames)} frames") -sorted = [0, 0, 0, 0, 0] -for i in range(len(dataset.frames)): - sorted[dataset.frames[i].label] += 1 -print(F'Stable {sorted[0]} Front {sorted[1]} Back {sorted[2]} Left {sorted[3]} Right {sorted[4]}') - -time_to_remove = input("Remove how many seconds: ") -try: - frames_to_remove = int(float(time_to_remove) * frequency) - print(F"{frames_to_remove} ---") - dataset.frames = dataset.frames[:-frames_to_remove - 1] - print(F"Frames after removing: {len(dataset.frames)}") -except: - pass - -reduce_to = input("Reduce number to ") -if reduce_to != "": - dataset.frames = random.choices(dataset.frames, k=int(reduce_to)) - print(F"Reduced number of frames to {len(dataset.frames)}") - -file_name = input("File name: ") -if len(file_name) > 0: - with open(file_name + ".pkl", "wb") as file: - pickle.dump(dataset, file) - - print(F"Saved dataset with {len(dataset.frames)} frames to {file_name}.pkl") diff --git a/bitbots_hcm/bitbots_hcm/training/train.py b/bitbots_hcm/bitbots_hcm/training/train.py deleted file mode 100644 index a1dab1c2a..000000000 --- a/bitbots_hcm/bitbots_hcm/training/train.py +++ /dev/null @@ -1,219 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import math -import pickle -import random -from pathlib import Path -from os import listdir -from os.path import isfile, join - -import optuna -import numpy as np -import sklearn -from optuna.samplers import TPESampler -from sklearn.gaussian_process import GaussianProcessClassifier -from sklearn.metrics import confusion_matrix, accuracy_score -from sklearn.model_selection import train_test_split -from sklearn.neural_network import MLPClassifier - -from bitbots_hcm.training.dataset import Dataset -from bitbots_hcm.fall_checker import FallChecker - -parser = argparse.ArgumentParser() -parser.add_argument('--storage', help='Database SQLAlchemy string, e.g. postgresql://USER:PASS@SERVER/DB_NAME', - default=None, type=str, required=False) -parser.add_argument('--name', help='Name of the study', default=None, type=str, required=False) -parser.add_argument('--iterations', help='iterations for optimization', default=100, type=int, required=False) -parser.add_argument('--knn', help='use knn', dest='knn', action='store_true') -parser.add_argument('--svc', help='use svc', dest='svc', action='store_true') -parser.add_argument('--mlp', help='use mlp', dest='mlp', action='store_true') -parser.add_argument('--gpc', help='use gpc', dest='gpc', action='store_true') -parser.add_argument('--bb', help='use bb', dest='bb', action='store_true') -parser.add_argument('--reduce', help='reduce data', dest='reduce', action='store_true') -parser.add_argument('--directories', help='directories of labelled training data', nargs='+', required=True) -args = parser.parse_args() - -# load dataset -merged_dataset = Dataset() -for directory in args.directories: - data_files = [f for f in listdir(directory) if isfile(join(directory, f))] - print(F"Loaded datasets {data_files}") - for file_name in data_files: - with open(directory + "/" + file_name, 'rb') as file: - dataset: Dataset = pickle.load(file) - merged_dataset.append_dataset(dataset) - -data_types = {'imu_raw': True, 'imu_orient': True, 'joint_states': False, 'imu_fused': False, 'cop': False} -print(f"Used datatypes are: {data_types}\n Change this in the script if you want to use other combinations.") - -x, y = merged_dataset.as_scikit_data(imu_raw=data_types['imu_raw'], imu_orient=data_types['imu_orient'], - joint_states=data_types['joint_states'], imu_fused=data_types['imu_fused'], - cop=data_types['cop']) - -print("Transformed to scipy data") -print(F"stable: {y.count(0)} front: {y.count(1)} back: {y.count(2)} left: {y.count(3)} right: {y.count(4)}") - - -def reduce_data(x, y, max_number=np.inf): - lowest_number = np.inf - for i in range(5): - count = y.count(i) - lowest_number = min(lowest_number, count) - lowest_number = min(lowest_number, max_number) - # make 5 lists with the indexes of frames for each class - sorted = [[], [], [], [], []] - for i in range(len(x)): - sorted[y[i]].append(i) - print(F'0 {len(sorted[0])} 1 {len(sorted[1])} 2 {len(sorted[2])} 3 {len(sorted[3])} 4 {len(sorted[4])}') - x_out = [] - y_out = [] - for i in range(5): - sampled_indexes = random.choices(sorted[i], k=lowest_number) - for index in sampled_indexes: - x_out.append(x[index]) - y_out.append(y[index]) - - return x_out, y_out - - -if args.reduce: - x, y = reduce_data(x, y, 200) -print(F'reduced to {y.count(0)} front: {y.count(1)} back: {y.count(2)} left: {y.count(3)} right: {y.count(4)}') -print(F'stable {y.count(0)} falling {y.count(1) + y.count(2) + y.count(3) + y.count(4)}') -scaler = sklearn.preprocessing.StandardScaler() -scaler.fit(x) - -use_fn = False -if use_fn: - x, x_test, y, y_test = train_test_split(x, y, random_state=0) - x_test_scaled = scaler.transform(x_test) - print(F'train is to {y.count(0)} front: {y.count(1)} back: {y.count(2)} left: {y.count(3)} right: {y.count(4)}') - print( - F'test is to {y_test.count(0)} front: {y_test.count(1)} back: {y_test.count(2)} left: {y_test.count(3)} right: {y_test.count(4)}') - -x_scaled = scaler.transform(x) - - -def evaluate_classifier(classifier): - if use_fn: - classifier.fit(x_scaled, y) - conf_mat = confusion_matrix(y_test, classifier.predict(x_test_scaled)) - FP = conf_mat.sum(axis=0) - np.diag(conf_mat) - FN = conf_mat.sum(axis=1) - np.diag(conf_mat) - TP = np.diag(conf_mat) - # TN = conf_mat.values.sum() - (FP + FN + TP) - print(FN) - return FN[0] * 100 + FN[1] + FN[2] + FN[3] + FN[4] - else: - score = sklearn.model_selection.cross_val_score(classifier, x_scaled, y, n_jobs=-1, verbose=0) - accuracy = score.mean() - return accuracy - - -def knn(trial: optuna.Trial): - n_neighbors = trial.suggest_int("n_neighbors", 1, 10) - weights = trial.suggest_categorical("weights", ['uniform', 'distance']) - algorithm = trial.suggest_categorical("algorithm", ['ball_tree', 'kd_tree']) - leaf_size = trial.suggest_int('leaf_size', 1, 50) - classifier = sklearn.neighbors.KNeighborsClassifier(n_neighbors=n_neighbors, weights=weights, algorithm=algorithm, - leaf_size=leaf_size) - return evaluate_classifier(classifier) - - -def svc(trial: optuna.Trial): - svc_c = trial.suggest_loguniform("svc_c", 1e-10, 1e10) - kernel = trial.suggest_categorical('kernel', ['linear', 'poly', 'rbf', 'sigmoid']) - degree = trial.suggest_int('degree', 1, 10) - classifier = sklearn.svm.SVC(C=svc_c, gamma="auto", kernel=kernel, degree=degree) - return evaluate_classifier(classifier) - - -def mlp(trial: optuna.Trial): - hidden_layer_size = trial.suggest_int("hidden_layer_size", 10, 1000, 10) - hidden_layer_count = trial.suggest_int("hidden_layer_count", 1, 10) - hidden_layer = (hidden_layer_size,) * hidden_layer_count - activation_fun = trial.suggest_categorical("activation_fun", ['identity', 'tanh', 'relu']) - alpha = trial.suggest_float("alpha", 0.000001, 0.0001) - learning_rate = trial.suggest_categorical('learning_rate', ['constant', 'invscaling', 'adaptive']) - classifier = MLPClassifier(hidden_layer_sizes=hidden_layer, activation=activation_fun, - alpha=alpha, learning_rate=learning_rate) - return evaluate_classifier(classifier) - - -def gpc(trial: optuna.Trial): - multi_class = trial.suggest_categorical("multi_class", ['one_vs_rest', 'one_vs_one']) - classifier = GaussianProcessClassifier(multi_class=multi_class) - return evaluate_classifier(classifier) - - -def bb(trial: optuna.Trial): - # our previous naive bitbots approach - thresh_gyro_front = trial.suggest_float("thresh_gyro_front", 0, 20) - thresh_gyro_side = trial.suggest_float("thresh_gyro_side", 0, 20) - thresh_orient_front = trial.suggest_float("thresh_orient_front", 0, math.pi) - thresh_orient_side = trial.suggest_float("thresh_orient_side", 0, math.pi) - classifier = FallChecker(thresh_gyro_front=thresh_gyro_front, thresh_gyro_side=thresh_gyro_side, - thresh_orient_front=thresh_orient_front, thresh_orient_side=thresh_orient_side) - return evaluate_classifier(classifier) - - -def pickle_classifier(classifier, type): - path = "classifiers/" + type + "/" - Path(path).mkdir(parents=True, exist_ok=True) - with open(path + "classifier.pkl", "wb") as file: - pickle.dump(classifier, file) - with open(path + "scaler.pkl", "wb") as file: - pickle.dump(scaler, file) - with open(path + "types.pkl", "wb") as file: - pickle.dump(data_types, file) - - -if args.knn: - objective = knn - name = 'knn' -elif args.mlp: - objective = mlp - name = 'mlp' -elif args.svc: - objective = svc - name = 'svc' -elif args.gpc: - objective = gpc - name = 'gpc' -elif args.bb: - objective = bb - name = 'bb' - -seed = np.random.randint(2 ** 32 - 1) -sampler = TPESampler(n_startup_trials=10, seed=seed) -if use_fn: - direction = 'minimize' -else: - direction = 'maximize' -study = optuna.create_study(study_name=args.name, storage=args.storage, direction=direction, - sampler=sampler, load_if_exists=True) -print("Start optimizing hyper parameters") -study.optimize(objective, n_trials=args.iterations, show_progress_bar=True, n_jobs=1) - -print("Train final classifier with optimized hyper parameters") -if args.knn: - classifier = sklearn.neighbors.KNeighborsClassifier(n_neighbors=study.best_params['n_neighbors'], - weights=study.best_params['weights'], - algorithm=study.best_params['algorithm'], - leaf_size=study.best_params['leaf_size']) -elif args.mlp: - hidden_layer = (study.best_params['hidden_layer_size'],) * study.best_params['hidden_layer_count'] - classifier = MLPClassifier(hidden_layer_sizes=hidden_layer, activation=study.best_params['activation_fun'], - alpha=study.best_params['alpha'], learning_rate=study.best_params['learning_rate']) -elif args.svc: - classifier = sklearn.svm.SVC(C=study.best_params['svc_c'], gamma="auto", kernel=study.best_params['kernel'], - degree=study.best_params['degree']) -elif args.gpc: - classifier = GaussianProcessClassifier(multi_class=study.best_params['multi_class']) -elif args.bb: - classifier = FallChecker(thresh_gyro_front=study.best_params['thresh_gyro_front'], - thresh_gyro_side=study.best_params['thresh_gyro_side'], - thresh_orient_front=study.best_params['thresh_orient_front'], - thresh_orient_side=study.best_params['thresh_orient_side']) -classifier.fit(x_scaled, y) -pickle_classifier(classifier, name) diff --git a/bitbots_hcm/config/hcm_wolfgang.yaml b/bitbots_hcm/config/hcm_wolfgang.yaml index 9fcd20423..422a9f8f9 100644 --- a/bitbots_hcm/config/hcm_wolfgang.yaml +++ b/bitbots_hcm/config/hcm_wolfgang.yaml @@ -1,62 +1,40 @@ /**: ros__parameters: - #Pressure sensors + # Pressure sensors pressure_sensors_installed: False - foot_zero_service: "set_foot_zero" + + # IMU + imu_timeout_duration: 0.5 # time without messages from the IMU till error is produced [s] # Motors motor_off_time: 30000000.0 # time of no use or updates when the hcm goes to soft off motor_timeout_duration: 0.5 # time without messages from the servos till error is produced [s] - imu_timeout_duration: 0.5 # time without messages from the IMU till error is produced [s] - - #Animations - anim_server_wait_time: 1.0 # time the hcm waits for animation server + # Animations + anim_server_wait_time: 10.0 # time the hcm waits for animation server animations: - walkready: "walkready" - walkready_sim: "walkready_sim" falling_front: "falling_front" falling_back: "falling_back" falling_left: "falling_left" falling_right: "falling_right" - stand_up_front: "stand_up_front" - stand_up_back: "stand_up_back" - stand_up_left: "turning_back_left" - stand_up_right: "turning_back_right" - sit_down: "sit" - penalty: "walkready" - motor_off: "sit" - init: "init" - anim_package: "wolfgang_animations" - walkready_pose_threshold: 50.0 # [deg] + turning_back_left: "turning_back_left" + turning_back_right: "turning_back_right" # Falling - stand_up_active: true # enables the robot to stand up automatically - falling_active: true # enables the check for falling and coresponding counter meassurements - #Grenzwerte schärfer --> fruehere Reaktion aber haeufigere Fehlinterpretation, zb. beim Laufen - #Grenzwerte sanfter --> zu spaete Raktion + stand_up_active: true # Enables the robot to stand up automatically + falling_active: true # Enables the check for falling and corresponding counter measurements + # Threshold tighter --> earlier reaction but more false positives, e.g. while walking + # Threshold softer --> too late reaction falling_thresh_gyro_pitch: 7.0 # > gyroY falling_thresh_gyro_roll: 7.0 # > gyroX falling_thresh_orient_roll: 60.0 # > Point of no return in degrees falling_thresh_orient_pitch: 45.0 # > Point of no return in degrees - # smoothing threshold for classifier + # Duration in seconds in which the robot has to be in a falling state to trigger the fall smooth_threshold: 0.04 - pick_up_accel_threshold: 7.0 - - falling: - - #falling animations - "falling_front": "falling_front" - "falling_back": "falling_back" - "falling_left": "falling_left" - "falling_right": "falling_right" - - "wolfgang": + # Fallen + fallen_orientation_thresh: 60.0 # Lean (degrees) after which we consider the robot fallen on this side + fallen_angular_velocity_thresh: 0.2 # Angular velocity (rad/s) under which we consider the robot as not moving (the fall is over and we are lying on the ground) - #Grenzwerte müssen auf härterem boden auch härter eingestellt werden, - #dies erfolgt mit dem boden_koeffizient - "ground_coefficient": 1.0 #WEICH(kunstrasen Spielfeld)= 1.0 , HART(teppich Spielfeld)= 1.25 - - #false = motoren werden bei fallerkennung abgeschaltet - "dyn_falling_active": true + # Pick up + pick_up_accel_threshold: 7.0 diff --git a/bitbots_hcm/docs/index.rst b/bitbots_hcm/docs/index.rst index 3398e2ed4..ccea79bcf 100644 --- a/bitbots_hcm/docs/index.rst +++ b/bitbots_hcm/docs/index.rst @@ -53,7 +53,7 @@ How the HCM works The HCM uses a DSD to decide what the state of the robot is and to decide on the actions that should be performed. This state is then published as a ROS message (`/robot_state`). -To see which state is which, you have to look at the message definition (`rosmsg show humanoid_league_msgs/RobotControlState`). +To see which state is which, you have to look at the message definition (`ros2 interface show bitbots_msgs/msg/RobotControlState`). The HCM subscribes to all joint goal topics of the different software parts. Dependend on its state, it forwards the goals or not. @@ -62,29 +62,19 @@ Sensor data is not influenced by the HCM, since it does not need to be mutexed. How the HCM is started ---------------------- -The easiest way to start the HCM is to launch the complete motion (`roslaunch bitbots_bringup motion_standalone.launch`). +The easiest way to start the HCM is to launch the complete motion (`ros2 launch bitbots_bringup motion_standalone.launch`). For debugging it is sometimes better to launch the single parts by themselves. -The HCM needs the animation server (`roslaunch bitbots_animation_server animation.launch`) to work because it is needed to perform falling and stand up animations. -To be able to actually control the hardware, ros_control needs to run (`roslaunch bitbots_ros_control ros_control_standalone.launch`). -Finally launch the HCM itself (`roslaunch bitbots_hcm hcm_standalone.launch`). - -How to retrain the falling classification ------------------------------------------ - -All necessary scripts are in the `training folder`. Previous labeled datasets can be found `here `_. - -The `label.py` script allows to label either online on a robot or on a prerecorded rosbag. - -The `train.py` script allows to train the classifier. - +The HCM needs the animation server (`ros2 launch bitbots_animation_server animation.launch`) to work because it is needed to perform falling and stand up animations. +To be able to actually control the hardware, ros_control needs to run (`ros2 launch bitbots_ros_control ros_control_standalone.launch`). +Finally launch the HCM itself (`ros2 launch bitbots_hcm hcm_standalone.launch`). What to do when it does not work -------------------------------- 1. Is `ros_control` running? Do you recieve joint states (`/joint_states`) or IMU data (`/imu/data_raw`)? -2. What is the state of the HCM (`rostopic echo /robot_state`)? The number has to be matched with the message - description (`rosmsg show humanoid_league_msgs/RobotControlState`). +2. What is the state of the HCM (`ros2 topic echo /robot_state`)? The number has to be matched with the message + description (`ros2 interface show bitbots_msgs/msg/RobotControlState`). 3. The visualization of the DSD is possible with the standard DSD visualization using the rqt plugin. There you can see exactly which decision is responsible for the current behavior and then you can look into the code. diff --git a/bitbots_hcm/launch/test.launch b/bitbots_hcm/launch/test.launch index 262385450..6fc8e1bc4 100644 --- a/bitbots_hcm/launch/test.launch +++ b/bitbots_hcm/launch/test.launch @@ -1,7 +1,11 @@ - + + + + + diff --git a/bitbots_hcm/package.xml b/bitbots_hcm/package.xml index 48f0b36c5..40e25f4d0 100644 --- a/bitbots_hcm/package.xml +++ b/bitbots_hcm/package.xml @@ -20,16 +20,19 @@ backward_ros bitbots_docs bitbots_msgs + bitbots_robot_description + bitbots_utils + dynamic_stack_decider geometry_msgs - humanoid_league_msgs + humanoid_league_speaker pybind11-dev + python3-numpy + python3-transforms3d rclpy + ros2_numpy ros2_python_extension sensor_msgs std_msgs - python3-sklearn - python3-transforms3d - python3-numpy diff --git a/bitbots_hcm/scripts/hcm_led.py b/bitbots_hcm/scripts/hcm_led.py index 92672d7fc..9b3d49fd7 100755 --- a/bitbots_hcm/scripts/hcm_led.py +++ b/bitbots_hcm/scripts/hcm_led.py @@ -3,86 +3,56 @@ import rclpy from rclpy.node import Node from std_msgs.msg import ColorRGBA -from humanoid_league_msgs.msg import RobotControlState + +from bitbots_msgs.msg import RobotControlState + BLINK_DURATION = 0.2 ERROR_TIMEOUT = 1 + class HCMLedNode(Node): def __init__(self): rclpy.init(args=None) self.pub = self.create_publisher(ColorRGBA, "/led1", 1) + self.color_mapping = { + RobotControlState.CONTROLLABLE: (0.3, 0.3, 0.3), + RobotControlState.FALLING: (0.3, 0.1, 0), + RobotControlState.FALLEN: (0.3, 0.3, 0), + RobotControlState.HARDWARE_PROBLEM: (1, 0, 0), + RobotControlState.ANIMATION_RUNNING: (0, 0, 0.3), + RobotControlState.WALKING: (0, 0.3, 0), + RobotControlState.GETTING_UP: (0, 0.3, 0.3), + RobotControlState.HCM_OFF: (0, 0, 0), + RobotControlState.MOTOR_OFF: (0.03, 0.03, 0.03), + RobotControlState.KICKING: (0, 0, 0.1), + RobotControlState.PENALTY: (0.3, 0, 0.3), + RobotControlState.PENALTY_ANIMATION: (0.3, 0, 0.3), + RobotControlState.PICKED_UP: (0, 0.03, 0), + RobotControlState.RECORD: (0, 0.1, 0), + } + self.last_state = -1 - sub = self.create_subscription(RobotControlState, "robot_state", self.hcm_state_cb, 1) + self.sub = self.create_subscription(RobotControlState, "robot_state", self.hcm_state_cb, 1) rclpy.spin(self) - def hcm_state_cb(msg: RobotControlState): + def hcm_state_cb(self, msg: RobotControlState): state = msg.state - # only do something if state changes, to not spam messages + + # Only do something if state changes, to not spam messages if state == self.last_state: return self.last_state = state + led = ColorRGBA() led.a = 1.0 - if state == msg.CONTROLLABLE: - led.r = 0.3 - led.g = 0.3 - led.b = 0.3 - elif state == msg.FALLING: - led.r = 0.3 - led.g = 0.1 - led.b = 0 - elif state == msg.FALLEN: - led.r = 0.3 - led.g = 0.3 - led.b = 0 - elif state == msg.HARDWARE_PROBLEM: - led.r = 1 - led.g = 0 - led.b = 0 - elif state == msg.ANIMATION_RUNNING: - led.r = 0 - led.g = 0 - led.b = 0.3 - elif state == msg.WALKING: - led.r = 0 - led.g = 0.3 - led.b = 0 - elif state == msg.GETTING_UP: - led.r = 0 - led.g = 0.3 - led.b = 0.3 - elif state == msg.HCM_OFF: - led.r = 0 - led.g = 0 - led.b = 0 - elif state == msg.MOTOR_OFF: - led.r = 0.03 - led.g = 0.03 - led.b = 0.03 - elif state == msg.KICKING: - led.r = 0 - led.g = 0 - led.b = 0.1 - elif state == msg.PENALTY: - led.r = 0.3 - led.g = 0 - led.b = 0.3 - elif state == msg.PENALTY_ANIMATION: - led.r = 0.3 - led.g = 0 - led.b = 0.3 - elif state == msg.PICKED_UP: - led.r = 0 - led.g = 0.03 - led.b = 0 - elif state == msg.RECORD: - led.r = 0 - led.g = 0.1 - led.b = 0 - self.pub.publish(led) + if state in self.color_mapping: + led.r, led.g, led.b = self.color_mapping[state] + else: + self.get_logger().warn(f"Unknown state: {state}") + self.pub.publish(led) diff --git a/bitbots_hcm/scripts/pause.py b/bitbots_hcm/scripts/pause.py index 48adeccc6..5db912ffd 100755 --- a/bitbots_hcm/scripts/pause.py +++ b/bitbots_hcm/scripts/pause.py @@ -2,13 +2,11 @@ # -*- coding:utf-8 -*- import rclpy from rclpy.node import Node -from humanoid_league_msgs.msg import Audio from std_msgs.msg import Bool from bitbots_msgs.srv import ManualPenalize -from humanoid_league_msgs.msg import GameState +from bitbots_msgs.msg import Audio from humanoid_league_speaker.speaker import speak -from rclpy.executors import ExternalShutdownException class Pause(object): @@ -29,21 +27,14 @@ def __init__(self): self.pause_publisher = self.node.create_publisher(Bool, "pause", 10) # todo latch self.speak_publisher = self.node.create_publisher(Audio, "speak", 10) - while rclpy.ok(): - try: - rclpy.spin_once(self.node) - except (ExternalShutdownException, KeyboardInterrupt): - exit(0) + rclpy.spin(self.node) - def manual_update(self, req:ManualPenalize.Request, resp:ManualPenalize.Response): - if req.penalize == 0: - # off + def manual_update(self, req: ManualPenalize.Request, resp: ManualPenalize.Response): + if req.penalize == ManualPenalize.Request.OFF: self.penalty_manual = False - elif req.penalize == 1: - # on + elif req.penalize == ManualPenalize.Request.ON: self.penalty_manual = True - elif req.penalize == 2: - # switch + elif req.penalize == ManualPenalize.Request.SWITCH: self.penalty_manual = not self.penalty_manual else: self.node.get_logger().error("Manual penalize call with unspecified request") diff --git a/bitbots_hcm/setup.cfg b/bitbots_hcm/setup.cfg deleted file mode 100644 index 3578a0006..000000000 --- a/bitbots_hcm/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/bitbots_hcm -[install] -install_scripts=$base/lib/bitbots_hcm diff --git a/bitbots_hcm/setup.py b/bitbots_hcm/setup.py deleted file mode 100644 index fb9ace321..000000000 --- a/bitbots_hcm/setup.py +++ /dev/null @@ -1,39 +0,0 @@ -import glob -import os - -from setuptools import find_packages -from setuptools import setup - -package_name = 'bitbots_hcm' - - -setup( - name=package_name, - packages=find_packages(exclude=['test']), - data_files=[ - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/launch', - glob.glob('launch/*.launch')), - - ('share/' + package_name + '/config', - glob.glob('config/*')), - - ('share/' + package_name + '/actions', - glob.glob(package_name + '/hcm_dsd/actions/*.py')), - ('share/' + package_name + '/decisions' , - glob.glob(package_name + '/hcm_dsd/decisions/*.py')), - ('share/' + package_name, - [package_name+'/hcm_dsd/hcm.dsd']) - ], - scripts=[ - 'scripts/hcm_led.py', - ], - install_requires=[ - 'launch', - 'setuptools', - ], - include_package_data=True, - zip_safe=True, - keywords=['ROS'], - license='MIT', -) diff --git a/bitbots_hcm/src/hcm.cpp b/bitbots_hcm/src/hcm.cpp index fbd7b96ab..63ebe3548 100644 --- a/bitbots_hcm/src/hcm.cpp +++ b/bitbots_hcm/src/hcm.cpp @@ -3,17 +3,17 @@ #include #include #include +#include "bitbots_msgs/msg/animation.hpp" #include "bitbots_msgs/msg/foot_pressure.hpp" #include "bitbots_msgs/msg/joint_command.hpp" +#include "bitbots_msgs/msg/robot_control_state.hpp" +#include "builtin_interfaces/msg/time.hpp" #include "geometry_msgs/msg/point_stamped.hpp" -#include "humanoid_league_msgs/msg/animation.hpp" -#include "humanoid_league_msgs/msg/robot_control_state.hpp" -#include "sensor_msgs/msg/joint_state.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include +#include "sensor_msgs/msg/joint_state.hpp" #include "std_msgs/msg/header.hpp" #include +#include using std::placeholders::_1; @@ -26,37 +26,49 @@ class HCM_CPP : public rclcpp::Node { : Node("hcm_cpp", rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( true)) { - // these are provided by the launch and not in the yaml file therefore we need to handle them seperatly + + // These are provided by the launch and not in the yaml file therefore we need to handle them seperatly bool use_sim_time, simulation_active, visualization_active; this->get_parameter("use_sim_time", use_sim_time); this->get_parameter("simulation_active", simulation_active); this->get_parameter("visualization_active", visualization_active); - current_state_ = humanoid_league_msgs::msg::RobotControlState::STARTUP; + // HCM state + current_state_ = bitbots_msgs::msg::RobotControlState::STARTUP; + // Sensor states current_imu_ = sensor_msgs::msg::Imu(); current_pressure_left_ = bitbots_msgs::msg::FootPressure(); current_pressure_right_ = bitbots_msgs::msg::FootPressure(); - current_cop_left_ = geometry_msgs::msg::PointStamped(); - current_cop_right_ = geometry_msgs::msg::PointStamped(); current_joint_state_ = sensor_msgs::msg::JointState(); + + // Walking state last_walking_time_ = builtin_interfaces::msg::Time(); + + // Kick state + last_kick_time_ = builtin_interfaces::msg::Time(); + + // Animation state record_active_ = false; external_animation_running_ = false; animation_requested_ = false; last_animation_goal_time_ = builtin_interfaces::msg::Time(); - // from bitbots_hcm.humanoid_control_module import HardwareControlManager + // Initialize HCM logic + // Import Python module + // "from bitbots_hcm.humanoid_control_module import HardwareControlManager" auto hcm_module = py::module::import("bitbots_hcm.humanoid_control_module"); + + // Create HCM object // hcm = HardwareControlManager() hcm_py_ = hcm_module.attr("HardwareControlManager")(use_sim_time, simulation_active, visualization_active); - // create goal publisher + // Create publishers pub_controller_command_ = this->create_publisher("DynamixelController/command", 1); - pub_robot_state_ = this->create_publisher("robot_state", 1); + pub_robot_state_ = this->create_publisher("robot_state", 1); - // create subscriber motor goals - anim_sub_ = this->create_subscription( + // Create subscribers for goals + anim_sub_ = this->create_subscription( "animation", 1, std::bind(&HCM_CPP::animation_callback, this, _1)); dynup_sub_ = this->create_subscription( "dynup_motor_goals", 1, std::bind(&HCM_CPP::dynup_callback, this, _1)); @@ -69,64 +81,69 @@ class HCM_CPP : public rclcpp::Node { walk_sub_ = this->create_subscription( "walking_motor_goals", 1, std::bind(&HCM_CPP::walking_goal_callback, this, _1)); - // subscriber for high frequency topics + // Create subscriber for high frequency sensor data joint_state_sub_ = this->create_subscription( - "joint_states", 1, std::bind(&HCM_CPP::joint_state_callback, this, _1)); - cop_l_sub_ = this->create_subscription( - "cop_l", 1, std::bind(&HCM_CPP::cop_l_callback, this, _1)); - cop_r_sub_ = this->create_subscription( - "cop_r", 1, std::bind(&HCM_CPP::cop_l_callback, this, _1)); + "joint_states", 1, std::bind(&HCM_CPP::joint_state_callback, this, _1)); pressure_l_sub_ = this->create_subscription( - "foot_pressure_left/filtered", 1, std::bind(&HCM_CPP::pressure_l_callback, this, _1)); + "foot_pressure_left/filtered", 1, std::bind(&HCM_CPP::pressure_l_callback, this, _1)); pressure_r_sub_ = this->create_subscription( - "foot_pressure_right/filtered", 1, std::bind(&HCM_CPP::pressure_r_callback, this, _1)); - imu_sub_ = - this->create_subscription("imu/data", 1, std::bind(&HCM_CPP::imu_callback, this, _1)); + "foot_pressure_right/filtered", 1, std::bind(&HCM_CPP::pressure_r_callback, this, _1)); + imu_sub_ = this->create_subscription( + "imu/data", 1, std::bind(&HCM_CPP::imu_callback, this, _1)); } - void animation_callback(humanoid_league_msgs::msg::Animation msg) { + void animation_callback(bitbots_msgs::msg::Animation msg) { // The animation server is sending us goal positions for the next keyframe last_animation_goal_time_ = msg.header.stamp; + // Check if the message is an animation request if (msg.request) { RCLCPP_INFO(this->get_logger(), "Got Animation request. HCM will try to get controllable now."); - // animation has to wait - // dsd should try to become controllable + // Animation has to wait + // DSD should try to become controllable animation_requested_ = true; return; } + + // Check if the message is the start of an animation if (msg.first) { if (msg.hcm) { - // coming from ourselves - // we don't have to do anything, since we must be in the right state + // This was an animation from the DSD + // We don't have to do anything, since we must be in the right state } else { - // coming from outside - // check if we can run an animation now - if (current_state_ != humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE) { + // Coming from outside + // Check if we can run an animation now + if (current_state_ != bitbots_msgs::msg::RobotControlState::CONTROLLABLE) { RCLCPP_WARN(this->get_logger(), "HCM is not controllable, animation refused."); } else { - // we're already controllable, go to animation running + // We're already controllable, go to animation running external_animation_running_ = true; } } } + // Check if the message is the end of an animation if (msg.last) { if (msg.hcm) { // This was an animation from the DSD - // we don't have to do anything, since we must be in the right state + // We don't have to do anything, since we must be in the right state } else { - // this is the last frame, we want to tell the DSD that we're finished with the animations + // This is the last frame, we want to tell the DSD that we're finished with the animations external_animation_running_ = false; if (msg.position.points.size() == 0) { - // probably this was just to tell us we're finished + // 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 (msg.position.points.size() > 0 && current_state_ != humanoid_league_msgs::msg::RobotControlState::GETTING_UP) { + + // Forward joint positions to motors if there are any and we're in the right state + if (msg.position.points.size() > 0 && ( + current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE || + current_state_ == bitbots_msgs::msg::RobotControlState::ANIMATION_RUNNING || + current_state_ == bitbots_msgs::msg::RobotControlState::FALLING || + current_state_ == bitbots_msgs::msg::RobotControlState::FALLEN)) { bitbots_msgs::msg::JointCommand out_msg = bitbots_msgs::msg::JointCommand(); out_msg.positions = msg.position.points[0].positions; out_msg.joint_names = msg.position.joint_names; @@ -149,26 +166,31 @@ class HCM_CPP : public rclcpp::Node { } void dynup_callback(const bitbots_msgs::msg::JointCommand msg) { - if (current_state_ == humanoid_league_msgs::msg::RobotControlState::STARTUP || - current_state_ == humanoid_league_msgs::msg::RobotControlState::FALLEN || - current_state_ == humanoid_league_msgs::msg::RobotControlState::GETTING_UP || - current_state_ == humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE) { + if (current_state_ == bitbots_msgs::msg::RobotControlState::STARTUP || + current_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP || + current_state_ == bitbots_msgs::msg::RobotControlState::MOTOR_OFF || + current_state_ == bitbots_msgs::msg::RobotControlState::PICKED_UP || + current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE) { pub_controller_command_->publish(msg); } } + void head_goal_callback(const bitbots_msgs::msg::JointCommand msg) { - if (current_state_ == humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE || - current_state_ == humanoid_league_msgs::msg::RobotControlState::WALKING) { + if (current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE || + current_state_ == bitbots_msgs::msg::RobotControlState::WALKING) { pub_controller_command_->publish(msg); } } + void kick_goal_callback(const bitbots_msgs::msg::JointCommand msg) { - if (current_state_ == humanoid_league_msgs::msg::RobotControlState::KICKING || - current_state_ == humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE) { + last_kick_time_ = msg.header.stamp; + if (current_state_ == bitbots_msgs::msg::RobotControlState::KICKING || + current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE) { // we can perform a kick pub_controller_command_->publish(msg); } } + void record_goal_callback(const bitbots_msgs::msg::JointCommand msg) { if (msg.joint_names.size() == 0) { // record tells us that its finished @@ -178,10 +200,11 @@ class HCM_CPP : public rclcpp::Node { pub_controller_command_->publish(msg); } } + void walking_goal_callback(bitbots_msgs::msg::JointCommand msg) { last_walking_time_ = msg.header.stamp; - if (current_state_ == humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE || - current_state_ == humanoid_league_msgs::msg::RobotControlState::WALKING) { + if (current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE || + current_state_ == bitbots_msgs::msg::RobotControlState::WALKING) { pub_controller_command_->publish(msg); } } @@ -190,14 +213,6 @@ class HCM_CPP : public rclcpp::Node { current_joint_state_ = msg; } - void cop_l_callback(geometry_msgs::msg::PointStamped msg) { - current_cop_right_ = msg; - } - - void cop_r_callback(geometry_msgs::msg::PointStamped msg) { - current_cop_left_ = msg; - } - void pressure_l_callback(bitbots_msgs::msg::FootPressure msg) { current_pressure_left_ = msg; } @@ -210,50 +225,67 @@ class HCM_CPP : public rclcpp::Node { current_imu_ = msg; } - void loop() { - // update all input to the HCM + void tick() { + // Performs one tick of the HCM DSD + + // Pass all the data nessesary data to the python module hcm_py_.attr("set_imu")(ros2_python_extension::toPython(current_imu_)); hcm_py_.attr("set_pressure_left")(ros2_python_extension::toPython(current_pressure_left_)); hcm_py_.attr("set_pressure_right")(ros2_python_extension::toPython(current_pressure_right_)); - hcm_py_.attr("set_cop")(ros2_python_extension::toPython(current_cop_left_), false); - hcm_py_.attr("set_cop")(ros2_python_extension::toPython(current_cop_right_), true); - hcm_py_.attr("set_last_motor_update_time")(ros2_python_extension::toPython(current_joint_state_.header.stamp)); hcm_py_.attr("set_current_joint_state")(ros2_python_extension::toPython(current_joint_state_)); hcm_py_.attr("set_last_walking_goal_time")(ros2_python_extension::toPython(last_walking_time_)); + hcm_py_.attr("set_last_kick_goal_time")(ros2_python_extension::toPython(last_kick_time_)); hcm_py_.attr("set_record_active")(record_active_); hcm_py_.attr("set_external_animation_running")(external_animation_running_); hcm_py_.attr("set_animation_requested")(animation_requested_); hcm_py_.attr("set_last_animation_goal_time")(ros2_python_extension::toPython(last_animation_goal_time_)); - // run HCM - hcm_py_.attr("loop")(); - // update current HCM state for joint mutex + + // Run HCM Python DSD code + hcm_py_.attr("tick")(); + + // Pull the current robot state from the python module + // It is used to perform the joint mutex py::object result = hcm_py_.attr("get_state")(); current_state_ = result.cast(); - // publish current state - humanoid_league_msgs::msg::RobotControlState state_msg = humanoid_league_msgs::msg::RobotControlState(); + + // Publish current robot state + bitbots_msgs::msg::RobotControlState state_msg = bitbots_msgs::msg::RobotControlState(); state_msg.state = current_state_; pub_robot_state_->publish(state_msg); } private: + // Python interpreter py::scoped_interpreter python_; + // Python hcm module py::object hcm_py_; + // The current robot state int current_state_; + + // Sensor states sensor_msgs::msg::Imu current_imu_; bitbots_msgs::msg::FootPressure current_pressure_left_; bitbots_msgs::msg::FootPressure current_pressure_right_; - geometry_msgs::msg::PointStamped current_cop_left_; - geometry_msgs::msg::PointStamped current_cop_right_; sensor_msgs::msg::JointState current_joint_state_; + + // Walking state builtin_interfaces::msg::Time last_walking_time_; + + // Kick state + builtin_interfaces::msg::Time last_kick_time_; + + // Animation states bool record_active_; bool external_animation_running_; bool animation_requested_; builtin_interfaces::msg::Time last_animation_goal_time_; + // Publishers rclcpp::Publisher::SharedPtr pub_controller_command_; - rclcpp::Publisher::SharedPtr pub_robot_state_; - rclcpp::Subscription::SharedPtr anim_sub_; + rclcpp::Publisher::SharedPtr pub_robot_state_; + + // Subscribers + rclcpp::Subscription::SharedPtr anim_sub_; rclcpp::Subscription::SharedPtr dynup_sub_; rclcpp::Subscription::SharedPtr head_sub_; rclcpp::Subscription::SharedPtr kick_sub_; @@ -287,7 +319,7 @@ int main(int argc, char** argv) { auto current_time = node->get_clock()->now(); if (current_time > last_time) { last_time = current_time; - node->loop(); + node->tick(); rate.sleep(); } // really short sleep to not waste cpu time @@ -295,4 +327,4 @@ int main(int argc, char** argv) { } rclcpp::shutdown(); -} \ No newline at end of file +} diff --git a/bitbots_hcm/test/pytest/test_dsd_valid.py b/bitbots_hcm/test/pytest/test_dsd_valid.py new file mode 100644 index 000000000..7b445c1cd --- /dev/null +++ b/bitbots_hcm/test/pytest/test_dsd_valid.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +import glob +import os + +from ament_index_python import get_package_share_directory +from dynamic_stack_decider import DSD + + +def test_dsd_valid(): + # Create empty blackboard + DummyBlackboard = object + # Create DSD + dsd = DSD(DummyBlackboard()) + + # Find install path of package where the dsd and python files are located + dirname = os.path.join(get_package_share_directory("bitbots_hcm"), "hcm_dsd") + + # Register actions and decisions + dsd.register_actions(os.path.join(dirname, "actions")) + dsd.register_decisions(os.path.join(dirname, "decisions")) + + # Load all dsd files to check if they are valid\ + for dsd_file in glob.glob(os.path.join(dirname, "*.dsd")): + dsd.load_behavior(dsd_file) diff --git a/bitbots_head_mover/CMakeLists.txt b/bitbots_head_mover/CMakeLists.txt new file mode 100644 index 000000000..909e0c8ef --- /dev/null +++ b/bitbots_head_mover/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(bitbots_head_mover) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(bio_ik REQUIRED) +find_package(bio_ik_msgs REQUIRED) +find_package(bitbots_msgs REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(bitbots_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +set(CMAKE_BUILD_TYPE Debug) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +generate_parameter_library( + head_parameters # cmake target name for the parameter library + config/head_config.yml) + +add_executable(move_head src/move_head.cpp) +target_link_libraries(move_head + rclcpp::rclcpp + head_parameters +) + + +ament_target_dependencies( + move_head + backward_ros + bio_ik + bio_ik_msgs + bitbots_msgs + generate_parameter_library + moveit_core + moveit_msgs + moveit_ros_planning + moveit_ros_planning_interface + rclcpp + sensor_msgs + std_msgs + tf2_geometry_msgs + tf2_kdl + tf2_ros) + +install(TARGETS + move_head + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/bitbots_head_mover/config/head_config.yml b/bitbots_head_mover/config/head_config.yml new file mode 100644 index 000000000..ce35a1cc6 --- /dev/null +++ b/bitbots_head_mover/config/head_config.yml @@ -0,0 +1,250 @@ +move_head: + # Max values for the head position + max_acceleration_pan: + type: double + default_value: 14.0 + description: "Max acceleration for pan" + validation: + bounds<>: [-1, 20] + + max_acceleration_tilt: + type: double + default_value: 14.0 + description: "Max acceleration for tilt" + validation: + bounds<>: [-1, 20] + + max_pan: + type: double_array + default_value: [-2.35, 2.35] + description: "Max values for the head position (in radians)" + validation: + fixed_size<>: 2 + element_bounds<>: [-3.14, 3.14] + + max_tilt: + type: double_array + default_value: [-1.2, 0.2] + description: "Max values for the head position (in radians)" + validation: + fixed_size<>: 2 + + look_at: + tilt_speed: + type: double + default_value: 6.0 + description: "Tilt speed for the look at action" + validation: + bounds<>: [0.0, 8.0] + pan_speed: + type: double + default_value: 6.0 + description: "Pan speed for the look at action" + validation: + bounds<>: [0.0, 8.0] + + # Search pattern for ball + search_pattern: + # search pattern speed + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + # Max values for the search pattern + pan_max: + type: double_array + default_value: [135.0, -135.0] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [-10.0, -60.0] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + # Number of scan lines for the search pattern + scan_lines: + type: int + default_value: 2 + description: "Number of scan lines for the search pattern" + validation: + gt_eq<>: [1] + + # Reduces last scanline by that factor so that robot does not collide + reduce_last_scanline: + type: double + default_value: 0.2 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + search_pattern_penalty: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 1.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 2.0 + + pan_max: + type: double_array + default_value: [-30.0, 30.0] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [-7.0, -30.0] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.2 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + search_pattern_field_features: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + pan_max: + type: double_array + default_value: [-135.0, 135.0] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [-10.0, -60.0] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.2 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + front_search_pattern: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + pan_max: + type: double_array + default_value: [ 0.0, 0.0 ] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [ -10.0, -70.0 ] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.0 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + look_forward: + tilt_speed: + type: double + description: "Tilt speed for the search pattern" + default_value: 3.0 + + pan_speed: + type: double + description: "Pan speed for the search pattern" + default_value: 3.0 + + pan_max: + type: double_array + default_value: [ 0.0, 0.0 ] + description: "Maximum pan values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + tilt_max: + type: double_array + default_value: [ -7.0, -7.0 ] + description: "Maximum tilt values for the search pattern (in degrees)" + validation: + fixed_size<>: 2 + + scan_lines: + type: int + default_value: 2 + description: "Number of scan horizontal lines for the search pattern" + validation: + gt_eq<>: [1] + + reduce_last_scanline: + type: double + default_value: 0.0 + description: "Reduces last scanline by that factor so that robot does not collide" + validation: + bounds<>: [0.0, 1.0] + + position_reached_threshold: + type: double + default_value: 5.0 + description: "Threshold (in degrees) when a head position is reached and + the next position will be triggered" diff --git a/bitbots_head_mover/launch/head_mover.launch b/bitbots_head_mover/launch/head_mover.launch new file mode 100644 index 000000000..51a2aa9f6 --- /dev/null +++ b/bitbots_head_mover/launch/head_mover.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/bitbots_head_mover/launch/head_mover_standalone.launch b/bitbots_head_mover/launch/head_mover_standalone.launch new file mode 100644 index 000000000..912732901 --- /dev/null +++ b/bitbots_head_mover/launch/head_mover_standalone.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/bitbots_head_mover/launch/test.launch b/bitbots_head_mover/launch/test.launch new file mode 100644 index 000000000..ab05df59a --- /dev/null +++ b/bitbots_head_mover/launch/test.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bitbots_head_mover/package.xml b/bitbots_head_mover/package.xml new file mode 100644 index 000000000..bef187136 --- /dev/null +++ b/bitbots_head_mover/package.xml @@ -0,0 +1,40 @@ + + + + bitbots_head_mover + 1.0.0 + The bitbots_head_mover handles the head movement. + The body behavior sends commands for different head movement + patterns. It can also use an action to look at a specific + point on the field, e.g. to track the ball. + Valerie Bartel + Hamburg Bit-Bots + Hamburg Bit-Bots + + MIT + + ament_cmake + + backward_ros + bio_ik_msgs + bio_ik + bitbots_msgs + bitbots_robot_description + bitbots_utils + generate_parameter_library + moveit_core + moveit_ros_planning_interface + moveit_ros_planning + moveit_ros + rclcpp + sensor_msgs + std_msgs + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + ament_lint_common + + ament_cmake + + diff --git a/bitbots_head_mover/src/move_head.cpp b/bitbots_head_mover/src/move_head.cpp new file mode 100644 index 000000000..374dd335e --- /dev/null +++ b/bitbots_head_mover/src/move_head.cpp @@ -0,0 +1,977 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "head_parameters.hpp" + +#include +#include + +#include + +using std::placeholders::_1; +using namespace std::chrono_literals; + +namespace move_head { + +#define DEG_TO_RAD M_PI / 180 + +using LookAtGoal = bitbots_msgs::action::LookAt; +using LookAtGoalHandle = rclcpp_action::ServerGoalHandle; + +class HeadMover { + std::shared_ptr node_; + + // Declare subscriber + rclcpp::Subscription::SharedPtr + head_mode_subscriber_; + rclcpp::Subscription::SharedPtr + joint_state_subscriber_; + + // Declare publisher + rclcpp::Publisher::SharedPtr + position_publisher_; + + // Declare tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Declare variables + uint head_mode_; + std::shared_ptr current_joint_state_; + bitbots_msgs::msg::JointCommand pos_msg_; + geometry_msgs::msg::PoseWithCovarianceStamped tf_precision_pose_; + + // Declare robot model and planning scene for moveit + robot_model_loader::RobotModelLoaderPtr loader_; + moveit::core::RobotModelPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + moveit::core::RobotStatePtr collision_state_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + planning_scene::PlanningScenePtr planning_scene_; + + // Declare parameters and parameter listener + move_head::Params params_; + std::shared_ptr param_listener_; + + // Declare timer that executes the main loop + rclcpp::TimerBase::SharedPtr timer_; + + // Declare variable for the current search pattern + std::vector> pattern_; + // Store previous head mode + uint prev_head_mode_ = -1; + + // Declare variables for the current search patterns parameters + double threshold_ = 0; + int index_ = 0; + double pan_speed_ = 0; + double tilt_speed_ = 0; + + // Action server for the look at action + rclcpp_action::Server::SharedPtr action_server_; + bool action_running_ = false; + +public: + HeadMover() { + node_ = std::make_shared("head_mover"); + + // Initialize publisher for head motor goals + position_publisher_ = + node_->create_publisher( + "head_motor_goals", 10); + + // Initialize subscriber for head mode + head_mode_subscriber_ = + node_->create_subscription( + "head_mode", 10, + [this](const bitbots_msgs::msg::HeadMode::SharedPtr msg) { + head_mode_callback(msg); + }); + + // Initialize subscriber for the current joint states of the robot + joint_state_subscriber_ = + node_->create_subscription( + "joint_states", 10, + [this](const sensor_msgs::msg::JointState::SharedPtr msg) { + joint_state_callback(msg); + }); + + // Create parameter listener and load initial set of parameters + param_listener_ = std::make_shared(node_); + params_ = param_listener_->get_params(); + + // Create a seperate node for moveit, this way we can use rqt to change parameters, + // because some moveit parameters break the gui + auto moveit_node = std::make_shared("moveit_head_mover_node"); + + // Get the parameters from the move_group node + auto parameters_client = + std::make_shared(node_, "/move_group"); + while (!parameters_client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + node_->get_logger(), + "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO( + node_->get_logger(), + "service not available, waiting again..."); + } + + // Extract the robot_description + rcl_interfaces::msg::ListParametersResult parameter_list = + parameters_client->list_parameters( + {"robot_description_kinematics"}, + 10); + + // Set the robot description parameters in the moveit node + auto copied_parameters = + parameters_client->get_parameters(parameter_list.names); + for (auto & parameter : copied_parameters) { + moveit_node->declare_parameter( + parameter.get_name(), + parameter.get_type()); + moveit_node->set_parameter(parameter); + } + + + // Load robot description / robot model into moveit + std::string robot_description = "robot_description"; + loader_ = std::make_shared( + robot_model_loader::RobotModelLoader( + moveit_node, robot_description, + true)); + robot_model_ = loader_->getModel(); + if (!robot_model_) { + RCLCPP_ERROR( + node_->get_logger(), + "failed to load robot model %s. Did you start the " + "blackboard (bitbots_bringup base.launch)?", + robot_description.c_str()); + } + + // Recreate robot state + robot_state_.reset(new moveit::core::RobotState(robot_model_)); + robot_state_->setToDefaultValues(); + + // Recreate collision state + collision_state_.reset(new moveit::core::RobotState(robot_model_)); + collision_state_->setToDefaultValues(); + + // Get planning scene for collision checking + planning_scene_monitor_ = + std::make_shared( + moveit_node, loader_); + planning_scene_ = planning_scene_monitor_->getPlanningScene(); + if (!planning_scene_) { + RCLCPP_ERROR_ONCE( + node_->get_logger(), + "failed to connect to planning scene"); + } + + // Prepare the pos_msg with default values + pos_msg_.joint_names = {"HeadPan", "HeadTilt"}; + pos_msg_.positions = {0, 0}; + pos_msg_.velocities = {0, 0}; + pos_msg_.accelerations = {params_.max_acceleration_pan, + params_.max_acceleration_pan}; + pos_msg_.max_currents = {-1, -1}; + + // Create tf buffer and listener to update it + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Initialize variables + threshold_ = params_.position_reached_threshold * DEG_TO_RAD; + + // Initialize action server for look at action + action_server_ = rclcpp_action::create_server( + node_, "look_at_goal", + std::bind( + &HeadMover::handle_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&HeadMover::handle_cancel, this, std::placeholders::_1), + std::bind(&HeadMover::handle_accepted, this, std::placeholders::_1)); + + // Initialize timer for main loop + timer_ = rclcpp::create_timer( + node_, node_->get_clock(), 10ms, + [this] {behave();}); + } + + + /** + * @brief Callback used to update the head mode + */ + void head_mode_callback( + const bitbots_msgs::msg::HeadMode::SharedPtr msg) { + head_mode_ = msg->head_mode; + } + + + /** + * @brief Callback used to get updates of the current joint states of the robot + */ + void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) { + current_joint_state_ = msg; + } + + /*** + * @brief Handles the goal request for the look at action + * + * @param uuid + * @param goal + */ + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) { + + // Avoid unused parameter warning + (void)uuid; + RCLCPP_DEBUG(node_->get_logger(), "Received goal request"); + + // Bring the goal point into the planning frame + geometry_msgs::msg::PointStamped new_point; + try { + new_point = tf_buffer_->transform( + goal->look_at_position, + planning_scene_->getPlanningFrame(), + tf2::durationFromSec(0.9)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + node_->get_logger(), "Could not transform goal point: %s", + ex.what()); + return rclcpp_action::GoalResponse::REJECT; + } + + // Get the motor goals that are needed to look at the point + std::pair pan_tilt = + get_motor_goals_from_point(new_point.point); + + // Check whether the goal is in range pan and tilt wise + bool goal_not_in_range = + check_head_collision(pan_tilt.first, pan_tilt.second); + + // Check whether the action goal is valid and can be executed + if (action_running_ || goal_not_in_range || + !(params_.max_pan[0] < pan_tilt.first && + pan_tilt.first < params_.max_pan[1]) || + !(params_.max_tilt[0] < pan_tilt.second && + pan_tilt.second < params_.max_tilt[1])) + { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + /** + * @brief Handles the cancel request for the look at action + * + * @param goal_handle + * @return rclcpp_action::CancelResponse + */ + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle) { + // Avoid unused parameter warning + (void)goal_handle; + RCLCPP_INFO(node_->get_logger(), "Received request to cancel goal"); + // Set the action_running_ flag to false, so that the action can be executed again + action_running_ = false; + return rclcpp_action::CancelResponse::ACCEPT; + } + + /** + * @brief Handles the accepted request for the look at action + * + * @param goal_handle + */ + void handle_accepted(const std::shared_ptr goal_handle) { + // Spawn a new thread that executes the look at action until we reach the goal + std::thread{ + std::bind(&HeadMover::execute_look_at, this, std::placeholders::_1), + goal_handle} + .detach(); + } + + /** + * @brief Executes the look at action that looks at a specific point in a given frame until the goal is reached or the action is canceled + * + * @param goal_handle + */ + void execute_look_at(const std::shared_ptr goal_handle) { + // Yeah seems like we are executing the action + action_running_ = true; + + RCLCPP_INFO(node_->get_logger(), "Executing goal"); + + // Get the goal from the goal handle + const auto goal = goal_handle->get_goal(); + + // Create feedback and result messages + auto feedback = std::make_shared(); + // Flag that indicates whether the action was successful yet + bool success = false; + auto result = std::make_shared(); + + // Execute the action until we reach the goal or the action is canceled + while (!success && rclcpp::ok()) { + RCLCPP_INFO(node_->get_logger(), "Looking at point"); + + // Check if the action was canceled and if so, set the result accordingly + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + RCLCPP_INFO(node_->get_logger(), "Goal was canceled"); + return; + } + + // Look at the goal point + success = look_at(goal->look_at_position); + + // Publish feedback to the client + goal_handle->publish_feedback( + feedback); // TODO: currently feedback is empty + } + + // If we reach this point, the action was successful + if (rclcpp::ok()) { + result->success = true; + goal_handle->succeed(result); + RCLCPP_INFO(node_->get_logger(), "Goal succeeded"); + } + + // Set the action_running_ flag to false, so that the action can be executed again + action_running_ = false; + } + + + /** + * @brief Slows down the speed of the joint that needs to travel less distance so both joints reach the goal at the same time + * + * @param delta_faster_joint The delta of the joint that needs to travel less distance and therefore reaches the goal faster + * @param delta_joint The delta of the joint that needs to travel more distance and therefore reaches the goal slower + * @param speed The maximum speed of the faster joint (the joint that needs to travel less distance) + * @return double The adjusted speed of the faster joint + */ + double calculate_lower_speed( + double delta_faster_joint, + double delta_joint, + double speed) { + double estimated_time = delta_faster_joint / speed; + if (estimated_time != 0) { + return delta_joint / estimated_time; + } else { + return 0; + } + } + + + /** + * @brief Send the goal positions to the head motors, but resolve collisions with the body if necessary. + * + */ + bool send_motor_goals( + double pan_position, + double tilt_position, + bool resolve_collision, + double pan_speed = 1.5, + double tilt_speed = 1.5, + double current_pan_position = 0.0, + double current_tilt_position = 0.0, + bool clip = true) { + + // Debug log the target pan and tilt position + RCLCPP_DEBUG_STREAM( + node_->get_logger(), + "target pan/tilt: " << pan_position << "/" << tilt_position); + + // Clip the target pan and tilt position at the maximum pan and tilt values as defined in the parameters + if (clip) { + std::tie(pan_position, tilt_position) = pre_clip(pan_position, tilt_position); + } + + // Resolve collisions if necessary + if (resolve_collision) { + // Call behavior that resolves collisions and might change the target pan and tilt position + bool success = avoid_collision_on_path( + pan_position, tilt_position, current_pan_position, + current_tilt_position, pan_speed, tilt_speed); + // Report error message of we were not able to move to an alternative collision free position + if (!success) { + RCLCPP_ERROR_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), + 1000, "Unable to resolve head collision"); + } + return success; + } else { + // Move the head to the target position but adjust the speed of the joints so both reach the goal at the same time + move_head_to_position_with_speed_adjustment( + pan_position, tilt_position, current_pan_position, + current_tilt_position, pan_speed, tilt_speed); + return true; + } + } + + /** + * @brief Applies clipping to the pan and tilt values based on the loaded config parameters + * + */ + std::pair pre_clip(double pan, double tilt) { + double new_pan = std::clamp(pan, params_.max_pan[0], params_.max_pan[1]); + double new_tilt = + std::clamp(tilt, params_.max_tilt[0], params_.max_tilt[1]); + return {new_pan, new_tilt}; + } + + /** + * @brief Tries to move the head to the target position but resolves collisions with the body if necessary. + * + */ + bool avoid_collision_on_path( + double goal_pan, + double goal_tilt, + double current_pan, + double current_tilt, + double pan_speed, + double tilt_speed, + int max_depth = 4, + int depth = 0) { + + // Check if we reached the maximum depth of the recursion and if so, return false + if (depth > max_depth) { + return false; + } + + // Calculate the distance between the current and the goal position + double distance = + sqrt(pow(goal_pan - current_pan, 2) - pow(goal_tilt - current_tilt, 2)); + + // Calculate the number of steps we need to take to reach the goal position + // This assumes that we move 3 degrees per step + int step_count = distance / (3 * DEG_TO_RAD); + + // Calculate path by performing linear interpolation between the current and the goal position + std::vector> pan_and_tilt_steps; + for (int i = 0; i < step_count; i++) { + pan_and_tilt_steps.push_back({ + current_pan + (goal_pan - current_pan) / step_count * i, + current_tilt + (goal_tilt - current_tilt) / step_count * i + }); + } + + // Check if we have collisions on our path + for (int i = 0; i < step_count; i++) { + if (check_head_collision( + pan_and_tilt_steps[i].first, + pan_and_tilt_steps[i].second)) + { + // If we have a collision, try to move the head to an alternative position + // The new position looks 10 degrees further up and is less likely to have a collision with the body + // Also increase the depth of the recursion as this is a new attempt to move the head to the goal position + return avoid_collision_on_path( + goal_pan, goal_tilt + 10 * DEG_TO_RAD, + current_pan, current_tilt, pan_speed, + tilt_speed, max_depth, depth + 1); + } + } + + // We do not have any collisions on our path, so we can move the head to the goal position + move_head_to_position_with_speed_adjustment( + goal_pan, goal_tilt, current_pan, current_tilt, pan_speed, tilt_speed); + return true; + } + + /** + * @brief Checks if the head collides with the body at a given pan and tilt position + */ + bool check_head_collision(double pan, double tilt) { + collision_state_->setJointPositions("HeadPan", &pan); + collision_state_->setJointPositions("HeadTilt", &tilt); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + collision_detection::AllowedCollisionMatrix acm = + planning_scene_->getAllowedCollisionMatrix(); + planning_scene_->checkCollision(req, res, *collision_state_, acm); + return res.collision; + } + + /** + * @brief Move the head to the target position but adjust the speed of the joints so both reach the goal at the same time + */ + void move_head_to_position_with_speed_adjustment( + double goal_pan, + double goal_tilt, + double current_pan, + double current_tilt, + double pan_speed, + double tilt_speed) { + // Calculate the delta between the current and the goal positions + double delta_pan = std::abs(goal_pan - current_pan); + double delta_tilt = std::abs(goal_tilt - current_tilt); + // Check which axis has to move further and adjust the speed of the other axis so both reach the goal at the same time + if (delta_pan > delta_tilt) { + // Slow down the tilt axis to match the time it takes for the pan axis to reach the goal + tilt_speed = std::min( + tilt_speed, calculate_lower_speed(delta_pan, delta_tilt, pan_speed)); + } else { + // Slow down the pan axis to match the time it takes for the tilt axis to reach the goal + pan_speed = std::min( + pan_speed, calculate_lower_speed(delta_tilt, delta_pan, tilt_speed)); + } + + // Send the motor goals including the position, speed and acceleration + pos_msg_.positions = {goal_pan, goal_tilt}; + pos_msg_.velocities = {pan_speed, tilt_speed}; + pos_msg_.accelerations = {params_.max_acceleration_pan, + params_.max_acceleration_pan}; + + pos_msg_.header.stamp = rclcpp::Clock().now(); + position_publisher_->publish(pos_msg_); + } + + /** + * @brief Returns the current position of the head motors + */ + std::pair get_head_position() { + double head_pan = 0.0; + double head_tilt = 0.0; + + // Iterate over all joints and find the head pan and tilt joints + for (size_t i = 0; i < current_joint_state_->name.size(); i++) { + if (current_joint_state_->name[i] == "HeadPan") { + head_pan = current_joint_state_->position[i]; + } else if (current_joint_state_->name[i] == "HeadTilt") { + head_tilt = current_joint_state_->position[i]; + } + } + return {head_pan, head_tilt}; + } + + + /** + * @brief Converts a scanline number to a tilt angle + */ + double lineAngle( + int line, + int line_count, + double min_angle, + double max_angle) { + // Get the angular delta that is covered by the scanlines in the tilt axis + double delta = std::abs(max_angle - min_angle); + // Calculate the angular step size between two scanlines + double steps = delta / (line_count - 1); + // Calculate the tilt angle of the given scanline + return steps * line + min_angle; + } + + /** + * @brief Performs a linear interpolation between the min and max pan values and returns the interpolated steps + */ + std::vector> interpolatedSteps( + int steps, + double tilt, + double min_pan, + double max_pan) { + // Handle edge case where we do not need to interpolate + if (steps == 0) { + return {}; + } + // Add one to the step count as we need to include the min and max pan values + steps += 1; + // Create a vector that stores the interpolated steps + std::vector> output_points; + // Calculate the delta between the min and max pan values + double delta = std::abs(max_pan - min_pan); + // Calculate the step size between two interpolated steps + double step_size = delta / steps; + // Iterate over all steps and calculate the interpolated pan values + for (int i = 1; i <= steps; i++) { + double pan = min_pan + step_size * i; + output_points.emplace_back(pan, tilt); + } + return output_points; + } + + /** + * @brief Generates a parameterized search pattern + */ + std::vector> generatePattern( + int line_count, + double max_horizontal_angle_left, + double max_horizontal_angle_right, + double max_vertical_angle_up, + double max_vertical_angle_down, + int reduce_last_scanline = 0.2, // TODO: needs to be changed to 1 + int interpolation_steps = 0) { + // Store the keyframes of the search pattern + std::vector> keyframes; + // Store the state of the generation process + bool down_direction = false; // true = down, false = up + bool right_side = false; // true = right, false = left + bool right_direction = true; // true = moving right, false = moving left + int line = line_count - 1; + // Calculate the number of iterations that are needed to generate the search pattern + int iterations = std::max(line_count * 4 - 4, 2); + // Iterate over all iterations and generate the search pattern + for (int i = 0; i < iterations; i++) { + // Get the maximum pan values (left and right) for the current pan position + // Select the relevant one based on the current side we are on + double current_pan; + if (right_side) { + current_pan = max_horizontal_angle_right; + } else { + current_pan = max_horizontal_angle_left; + } + + // Get the current tilt angle based on the current line we are on + double current_tilt = lineAngle( + line, line_count, max_vertical_angle_down, + max_vertical_angle_up); + + // Store the keyframe + keyframes.push_back({current_pan, current_tilt}); + + // Check if we move horizontally or vertically in the pattern + if (right_side != right_direction) { + // We move horizontally, so we might need to interpolate between the current and the next keyframe + std::vector> interpolated_points = + interpolatedSteps( + interpolation_steps, current_tilt, + max_horizontal_angle_right, + max_horizontal_angle_left); + // Reverse the order of the interpolated points if we are moving to the right + if (right_direction) { + std::reverse(interpolated_points.begin(), interpolated_points.end()); + } + // Add the interpolated points to the keyframes + keyframes.insert( + keyframes.end(), interpolated_points.begin(), + interpolated_points.end()); + // Change the direction we are moving in + right_side = right_direction; + + } else { + // Change the horizontal direction we are moving in + right_side = !right_direction; + // Check if we reached the top or bottom of the pattern and need to change the direction + if (0 <= line && line <= line_count - 1) { + down_direction = !down_direction; + } + // Change the line we are on based on the direction we are moving in + if (down_direction) { + line -= 1; + } else { + line += 1; + } + } + } + + // Reduce the last scanline by a given factor + for (auto & keyframe : keyframes) { + if (keyframe.second == max_vertical_angle_down) { + keyframe = {keyframe.first * reduce_last_scanline, + max_vertical_angle_down}; + } + } + return keyframes; + } + + + /** + * @brief Calculates the motor goals that are needed to look at a given point using the inverse kinematics + */ + std::pair get_motor_goals_from_point( + geometry_msgs::msg::Point point) { + + // Create a new IK options object + bio_ik::BioIKKinematicsQueryOptions ik_options; + ik_options.return_approximate_solution = true; + ik_options.replace = true; + + // Create a new look at goal and set the target point as the position the camera link should look at + ik_options.goals.emplace_back(new bio_ik::LookAtGoal( + "camera", + {1.0, 0.0, 0.0}, + {point.x, point.y, point.z})); + + // Get the joint model group for the head + auto joint_model_group = robot_model_->getJointModelGroup("Head"); + + // Try to calculate the inverse kinematics + double timeout_seconds = 1.0; + bool success = robot_state_->setFromIK( + joint_model_group, EigenSTL::vector_Isometry3d(), + std::vector(), timeout_seconds, + moveit::core::GroupStateValidityCallbackFn(), ik_options); + robot_state_->update(); + + // Get the solution from the IK response + bio_ik_msgs::msg::IKResponse response; + moveit::core::robotStateToRobotStateMsg(*robot_state_, response.solution); + response.solution_fitness = ik_options.solution_fitness; + // Return the motor goals if the IK was successful + if (success) { + return {response.solution.joint_state.position[0], response.solution.joint_state.position[1]}; + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), + 1000, "BioIK failed with error code: " << response.error_code.val); + return {0.0, 0.0}; + } + } + + /** + * @brief Looks at a given point and returns true if the goal position was reached + */ + bool look_at( + geometry_msgs::msg::PointStamped point, + double min_pan_delta = 0.02, + double min_tilt_delta = 0.02) { + try { + // Transform the point into the planning frame + geometry_msgs::msg::PointStamped new_point = + tf_buffer_->transform( + point, planning_scene_->getPlanningFrame(), + tf2::durationFromSec(0.9)); + + // Get the motor goals that are needed to look at the point from the inverse kinematics + std::pair pan_tilt = + get_motor_goals_from_point(new_point.point); + // Get the current head position + std::pair current_pan_tilt = get_head_position(); + + // Check if we reached the goal position + if (std::abs(pan_tilt.first - current_pan_tilt.first) > min_pan_delta || + std::abs(pan_tilt.second - current_pan_tilt.second) > + min_tilt_delta) + { + // Send the motor goals to the head motors + send_motor_goals( + pan_tilt.first, pan_tilt.second, true, + params_.look_at.pan_speed, params_.look_at.tilt_speed); + // Return false as we did not reach the goal position yet + return false; + } + // Return true as we reached the goal position + return true; + } catch (tf2::TransformException & ex) { + // Report error message if we were not able to transform the point + RCLCPP_ERROR(node_->get_logger(), "Transform error: %s", ex.what()); + // We obviously did not reach the goal position + return false; + } + } + + /** + * @brief Returns the index of the pattern keypoint that is closest to the current head position + * + * @param pattern The search pattern + * @param pan The current pan position + * @param tilt The current tilt position + * @return int The index of the pattern keypoint that is closest to the current head position + */ + int get_near_pattern_position( + std::vector> pattern, + double pan, + double tilt) { + // Store the index and distance of the closest keypoint + std::pair min_distance_point = {10000.0, -1}; + // Iterate over all keypoints and calculate the distance to the current head position + for (size_t i = 0; i < pattern.size(); i++) { + // Calculate the cartesian distance between the current head position and the keypoint + double distance = std::sqrt( + std::pow(pattern[i].first - pan, 2) + + std::pow(pattern[i].second - tilt, 2)); + // Check if the distance is smaller than the current minimum distance + // and if so, update the minimum distance accordingly + if (distance < min_distance_point.first) { + min_distance_point.first = distance; + min_distance_point.second = i; + } + } + // Return the index of the closest keypoint + return min_distance_point.second; + } + + /** + * @brief Performs the search pattern that is currently loaded + */ + void perform_search_pattern() { + // Do not perform the search pattern if it is empty + if (pattern_.size() == 0) { + return; + } + // Wrap the index that points to the current keypoint around if necessary + index_ = index_ % int(pattern_.size()); + // Get the current keypoint and convert it to radians + double head_pan = pattern_[index_].first * DEG_TO_RAD; + double head_tilt = pattern_[index_].second * DEG_TO_RAD; + // Get the current head position + auto [current_head_pan, current_head_tilt] = get_head_position(); + + // Send the motor goals to the head motors + bool success = + send_motor_goals( + head_pan, head_tilt, true, pan_speed_, tilt_speed_, + current_head_pan, current_head_tilt); + + if (success) { + // Check if we reached the current keypoint and if so, increase the index + double distance_to_goal = + std::sqrt( + std::pow(head_pan - current_head_pan, 2) + + std::pow(head_tilt - current_head_tilt, 2)); + if (distance_to_goal <= threshold_) { + index_++; + } + } else { + // Skip the keypoint if we were not able to reach it (e.g. due to an unresolvable collision) + index_++; + } + } + + /** + * @brief Callback for the ticks of the main loop + */ + void behave() { + // Get the current head mode + uint curr_head_mode = head_mode_; + + // Pull the parameters from the parameter server + params_ = param_listener_->get_params(); + + // Check if the head mode changed and if so, update the search pattern + if (prev_head_mode_ != curr_head_mode) { + switch (curr_head_mode) { + case bitbots_msgs::msg::HeadMode::BALL_MODE: // 0 + pan_speed_ = params_.search_pattern.pan_speed; + tilt_speed_ = params_.search_pattern.tilt_speed; + pattern_ = + generatePattern( + params_.search_pattern.scan_lines, + params_.search_pattern.pan_max[0], + params_.search_pattern.pan_max[1], + params_.search_pattern.tilt_max[0], + params_.search_pattern.tilt_max[1], + params_.search_pattern.reduce_last_scanline); + break; + case bitbots_msgs::msg::HeadMode::BALL_MODE_PENALTY: // 11 + pan_speed_ = params_.search_pattern_penalty.pan_speed; + tilt_speed_ = params_.search_pattern_penalty.tilt_speed; + pattern_ = + generatePattern( + params_.search_pattern_penalty.scan_lines, + params_.search_pattern_penalty.pan_max[0], + params_.search_pattern_penalty.pan_max[1], + params_.search_pattern_penalty.tilt_max[0], + params_.search_pattern_penalty.tilt_max[1], + params_.search_pattern.reduce_last_scanline); + break; + + case bitbots_msgs::msg::HeadMode::FIELD_FEATURES: // 3 + pan_speed_ = params_.search_pattern_field_features.pan_speed; + tilt_speed_ = params_.search_pattern_field_features.tilt_speed; + pattern_ = + generatePattern( + params_.search_pattern_field_features.scan_lines, + params_.search_pattern_field_features.pan_max[0], + params_.search_pattern_field_features.pan_max[1], + params_.search_pattern_field_features.tilt_max[0], + params_.search_pattern_field_features.tilt_max[1], + params_.search_pattern.reduce_last_scanline); + break; + + case bitbots_msgs::msg::HeadMode::LOOK_FRONT: // 13 + pan_speed_ = params_.front_search_pattern.pan_speed; + tilt_speed_ = params_.front_search_pattern.tilt_speed; + pattern_ = + generatePattern( + params_.front_search_pattern.scan_lines, + params_.front_search_pattern.pan_max[0], + params_.front_search_pattern.pan_max[1], + params_.front_search_pattern.tilt_max[0], + params_.front_search_pattern.tilt_max[1], + params_.search_pattern.reduce_last_scanline); + break; + + case bitbots_msgs::msg::HeadMode::LOOK_FORWARD: // 7 + pan_speed_ = params_.look_forward.pan_speed; + tilt_speed_ = params_.look_forward.tilt_speed; + pattern_ = generatePattern( + params_.look_forward.scan_lines, params_.look_forward.pan_max[0], + params_.look_forward.pan_max[1], params_.look_forward.tilt_max[0], + params_.look_forward.tilt_max[1], + params_.search_pattern.reduce_last_scanline); + break; + + default: + return; + } + + // Get the current head position + std::pair head_position = get_head_position(); + + // Select the closest keypoint in the search pattern as a starting point + index_ = get_near_pattern_position( + pattern_, head_position.first, + head_position.second); + } + // Check if no look at action is running or if the head mode is DONT_MOVE + // if this is not the case, perform the search pattern + if (!action_running_ && + curr_head_mode != + bitbots_msgs::msg::HeadMode::DONT_MOVE) // here DONT_MOVE + // is implemented + { + // Store the current head mode as the previous head mode to detect changes + prev_head_mode_ = curr_head_mode; + // Execute the search pattern + perform_search_pattern(); + } + } + + /** + * @brief A getter that returns the node + */ + std::shared_ptr get_node() {return node_;} +}; +} // namespace move_head + + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + rclcpp::experimental::executors::EventsExecutor exec; + auto head_mover = std::make_shared(); + exec.add_node(head_mover->get_node()); + exec.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/bitbots_moveit_bindings/bitbots_moveit_bindings/__init__.py b/bitbots_moveit_bindings/bitbots_moveit_bindings/__init__.py index 92d1cdc54..3855b179a 100644 --- a/bitbots_moveit_bindings/bitbots_moveit_bindings/__init__.py +++ b/bitbots_moveit_bindings/bitbots_moveit_bindings/__init__.py @@ -1,10 +1,10 @@ +from bio_ik_msgs.msg import IKRequest +from bio_ik_msgs.srv import GetIK +from bitbots_moveit_bindings.libbitbots_moveit_bindings import \ + BitbotsMoveitBindings from moveit_msgs.srv import GetPositionFK, GetPositionIK -from bitbots_moveit_bindings.libbitbots_moveit_bindings import BitbotsMoveitBindings -from rclpy.serialization import serialize_message, deserialize_message from rcl_interfaces.msg import Parameter -from bio_ik_msgs.srv import GetIK -from bio_ik_msgs.msg import IKRequest -from sensor_msgs.msg import JointState +from rclpy.serialization import deserialize_message, serialize_message # this state is only used for IK and FK calls and does not listen to current joint_states ik_fk_state = None diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index 4bd17893b..52e3a64f6 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -6,10 +6,11 @@ if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif () -find_package(Eigen3 REQUIRED) find_package(ament_cmake REQUIRED) find_package(biped_interfaces REQUIRED) find_package(bitbots_docs REQUIRED) +find_package(bitbots_utils REQUIRED) +find_package(Eigen3 REQUIRED) find_package(generate_parameter_library REQUIRED) find_package(geometry_msgs REQUIRED) find_package(message_filters REQUIRED) @@ -43,38 +44,40 @@ target_link_libraries( ## Specify libraries to link a library or executable target against ament_target_dependencies( motion_odometry - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs + ament_cmake biped_interfaces - geometry_msgs bitbots_docs + bitbots_utils + generate_parameter_library + geometry_msgs + message_filters + nav_msgs + rclcpp rot_conv sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs tf2_ros - ament_cmake - generate_parameter_library - rclcpp ) ament_target_dependencies( odometry_fuser - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs + ament_cmake biped_interfaces - geometry_msgs bitbots_docs + bitbots_utils + Eigen3 + geometry_msgs + message_filters + nav_msgs + rclcpp rot_conv sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs tf2_ros - ament_cmake - rclcpp - Eigen3 ) enable_bitbots_docs() @@ -88,4 +91,7 @@ install(TARGETS odometry_fuser install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/bitbots_odometry/config/odometry_config_default.yaml b/bitbots_odometry/config/odometry_config_default.yaml new file mode 100644 index 000000000..55021f9ac --- /dev/null +++ b/bitbots_odometry/config/odometry_config_default.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 0.6 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 2456dfd30..9de619119 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -1,16 +1,18 @@ #include +#include "odometry_parameters.hpp" + +#include +#include +#include +#include #include #include -#include #include +#include +#include #include #include -#include -#include #include -#include -#include -#include "odometry_parameters.hpp" using std::placeholders::_1; @@ -25,15 +27,12 @@ class MotionOdometry : public rclcpp::Node { char current_support_state_; char previous_support_state_; rclcpp::Time current_support_state_time_{rclcpp::Time(0, 0, RCL_ROS_TIME)}; - sensor_msgs::msg::JointState current_joint_states_; nav_msgs::msg::Odometry current_odom_msg_; tf2::Transform odometry_to_support_foot_; - std::unique_ptr tf_buffer_; std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_; rclcpp::Publisher::SharedPtr pub_odometry_; rclcpp::Subscription::SharedPtr walk_support_state_sub_; rclcpp::Subscription::SharedPtr kick_support_state_sub_; - rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr odom_subscriber_; // Declare parameter listener and struct from the generate_parameter_library @@ -42,9 +41,9 @@ class MotionOdometry : public rclcpp::Node { motion_odometry::Params config_; void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); - void jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg); void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + std::shared_ptr tf_buffer_; std::unique_ptr br_; std::shared_ptr tf_listener_; rclcpp::Time foot_change_time_; diff --git a/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h b/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h index 6fd92317b..1c9a7a9b4 100644 --- a/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h +++ b/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h @@ -1,30 +1,33 @@ #include -#include -#include -#include +#include +#include +#include +#include #include +#include +#include +#include +#include #include +#include +#include +#include #include -#include -#include +#include #include +#include +#include #include -#include -#include #include +#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include -#include using std::placeholders::_1; +using bitbots_utils::wait_for_tf; typedef message_filters::sync_policies::ApproximateTime SyncPolicy; @@ -32,14 +35,14 @@ class OdometryFuser : public rclcpp::Node { public: OdometryFuser(); void loop(); - bool wait_for_tf(); private: sensor_msgs::msg::Imu imu_data_; nav_msgs::msg::Odometry odom_data_; - std::unique_ptr tf_buffer_; + std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; rclcpp::Time fused_time_; std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_, rotation_frame_, imu_frame_; + bool imu_data_received_ = false; rclcpp::Subscription::SharedPtr walk_support_state_sub_; rclcpp::Subscription::SharedPtr kick_support_state_sub_; diff --git a/bitbots_odometry/launch/odometry.launch b/bitbots_odometry/launch/odometry.launch index 545c07314..df13a2816 100644 --- a/bitbots_odometry/launch/odometry.launch +++ b/bitbots_odometry/launch/odometry.launch @@ -20,7 +20,6 @@ - diff --git a/bitbots_odometry/package.xml b/bitbots_odometry/package.xml index 3a93cbe77..c3db876be 100644 --- a/bitbots_odometry/package.xml +++ b/bitbots_odometry/package.xml @@ -14,16 +14,17 @@ Jasper Güldenstein ament_cmake - nav_msgs + biped_interfaces + bitbots_docs + bitbots_utils + generate_parameter_library message_filters + nav_msgs + rot_conv sensor_msgs + tf2_eigen tf2_geometry_msgs tf2 - tf2_eigen - rot_conv - bitbots_docs - biped_interfaces - generate_parameter_library diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index 9c469bab3..26231793c 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -5,7 +5,7 @@ namespace bitbots_odometry { MotionOdometry::MotionOdometry() : Node("MotionOdometry"), param_listener_(get_node_parameters_interface()) { - tf_buffer_ = std::make_unique(this->get_clock()); + tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_, this); br_ = std::make_unique(this); config_ = param_listener_.get_params(); @@ -19,9 +19,10 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), this->declare_parameter("odom_frame", "odom"); this->get_parameter("odom_frame", odom_frame_); - joint_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + joint_update_time_ = rclcpp::Time(0); current_support_state_ = -1; previous_support_state_ = -1; + walk_support_state_sub_ = this->create_subscription("walk_support_state", 1, @@ -32,10 +33,6 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), 1, std::bind(&MotionOdometry::supportCallback, this, _1)); - joint_state_sub_ = - this->create_subscription("joint_states", - 1, - std::bind(&MotionOdometry::jointStateCb, this, _1)); odom_subscriber_ = this->create_subscription("walk_engine_odometry", 1, @@ -52,125 +49,125 @@ MotionOdometry::MotionOdometry() : Node("MotionOdometry"), } void MotionOdometry::loop() { - rclcpp::Time cycle_start_time = this->now(); - config_ = param_listener_.get_params(); - - //check if joint states were received, otherwise we can't provide odometry - rclcpp::Duration joints_delta_t = this->now() - joint_update_time_; - if (joints_delta_t.seconds() > 0.1) { - // only warn if we did not just start as this results in unecessary warnings - if ((this->now() - start_time_).seconds() > 10) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30000, - "No joint states received. Will not provide odometry."); - } - } else { - // check if step finished, meaning left->right or right->left support. double support is skipped - // the support foot change is published when the joint goals for the last movements are published. - // it takes some time till the joints actually reach this position, this can create some offset - // but since we skip the double support phase, we basically take the timepoint when the double support phase is - // finished. This means both feet did not move and this should create no offset. - if ((current_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || - (current_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { - foot_change_time_ = current_support_state_time_; - if (previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE) { - previous_support_link_ = l_sole_frame_; - current_support_link_ = r_sole_frame_; - } else { - previous_support_link_ = r_sole_frame_; - current_support_link_ = l_sole_frame_; - } + // Wait for tf to be available + bitbots_utils::wait_for_tf( + this->get_logger(), + this->get_clock(), + tf_buffer_, + {base_link_frame_, r_sole_frame_, l_sole_frame_}, + base_link_frame_); - try { - // add the transform between previous and current support link to the odometry transform. - // we wait a bit for the transform as the joint messages are maybe a bit behind - geometry_msgs::msg::TransformStamped previous_to_current_support_msg = - tf_buffer_->lookupTransform(previous_support_link_, - current_support_link_, - foot_change_time_, - rclcpp::Duration::from_nanoseconds(0.1 * 1e9)); - tf2::Transform previous_to_current_support = tf2::Transform(); - tf2::fromMsg(previous_to_current_support_msg.transform, previous_to_current_support); - // setting translation in z axis, pitch and roll to zero to stop the robot from lifting up - // scale odometry based on parameters - double x = previous_to_current_support.getOrigin().x(); - if (x > 0) { - x = x * config_.x_forward_scaling; - } else { - x = x * config_.x_backward_scaling; - } - double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; - double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; - previous_to_current_support.setOrigin({x, y, 0}); - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - previous_to_current_support.setRotation(q); - odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; - } catch (tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - rclcpp::sleep_for(std::chrono::milliseconds(1000)); - return; - } - // update current support link for transform from foot to base link - previous_support_link_ = current_support_link_; + rclcpp::Time cycle_start_time = this->now(); + config_ = param_listener_.get_params(); - // remember the support state change but skip the double support phase - if (current_support_state_ != biped_interfaces::msg::Phase::DOUBLE_STANCE) { - previous_support_state_ = current_support_state_; - } + // check if step finished, meaning left->right or right->left support. double support is skipped + // the support foot change is published when the joint goals for the last movements are published. + // it takes some time till the joints actually reach this position, this can create some offset + // but since we skip the double support phase, we basically take the timepoint when the double support phase is + // finished. This means both feet did not move and this should create no offset. + if ((current_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE + && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || + (current_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE + && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { + foot_change_time_ = current_support_state_time_; + if (previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE) { + previous_support_link_ = l_sole_frame_; + current_support_link_ = r_sole_frame_; + } else { + previous_support_link_ = r_sole_frame_; + current_support_link_ = l_sole_frame_; } - //publish odometry and if wanted transform to base_link try { - geometry_msgs::msg::TransformStamped - current_support_to_base_msg = - tf_buffer_->lookupTransform(previous_support_link_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); - tf2::Transform current_support_to_base; - tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); - double x = current_support_to_base.getOrigin().x(); - if (current_odom_msg_.twist.twist.linear.x > 0) { + // add the transform between previous and current support link to the odometry transform. + // we wait a bit for the transform as the joint messages are maybe a bit behind + geometry_msgs::msg::TransformStamped previous_to_current_support_msg = + tf_buffer_->lookupTransform(previous_support_link_, + current_support_link_, + foot_change_time_, + rclcpp::Duration::from_nanoseconds(0.1 * 1e9)); + tf2::Transform previous_to_current_support = tf2::Transform(); + tf2::fromMsg(previous_to_current_support_msg.transform, previous_to_current_support); + // setting translation in z axis, pitch and roll to zero to stop the robot from lifting up + // scale odometry based on parameters + double x = previous_to_current_support.getOrigin().x(); + if (x > 0) { x = x * config_.x_forward_scaling; } else { x = x * config_.x_backward_scaling; } - double y = current_support_to_base.getOrigin().y() * config_.y_scaling; - double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; - current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); + double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; + previous_to_current_support.setOrigin({x, y, 0}); tf2::Quaternion q; q.setRPY(0, 0, yaw); - current_support_to_base.setRotation(q); - - tf2::Transform odom_to_base_link = odometry_to_support_foot_ * current_support_to_base; - geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); - odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_link); - odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; - odom_to_base_link_msg.header.frame_id = odom_frame_; - odom_to_base_link_msg.child_frame_id = base_link_frame_; - if (config_.publish_walk_odom_tf) { - RCLCPP_WARN_ONCE(this->get_logger(), "Sending Tf from walk odometry directly"); - br_->sendTransform(odom_to_base_link_msg); - } - - // odometry as message - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = current_support_to_base_msg.header.stamp; - odom_msg.header.frame_id = odom_frame_; - odom_msg.child_frame_id = base_link_frame_; - odom_msg.pose.pose.position.x = odom_to_base_link_msg.transform.translation.x; - odom_msg.pose.pose.position.y = odom_to_base_link_msg.transform.translation.y; - odom_msg.pose.pose.position.z = odom_to_base_link_msg.transform.translation.z; - odom_msg.pose.pose.orientation = odom_to_base_link_msg.transform.rotation; - odom_msg.twist = current_odom_msg_.twist; - pub_odometry_->publish(odom_msg); - + previous_to_current_support.setRotation(q); + odometry_to_support_foot_ = odometry_to_support_foot_ * previous_to_current_support; } catch (tf2::TransformException &ex) { RCLCPP_WARN(this->get_logger(), "%s", ex.what()); rclcpp::sleep_for(std::chrono::milliseconds(1000)); return; } + + // update current support link for transform from foot to base link + previous_support_link_ = current_support_link_; + + // remember the support state change but skip the double support phase + if (current_support_state_ != biped_interfaces::msg::Phase::DOUBLE_STANCE) { + previous_support_state_ = current_support_state_; + } } + + //publish odometry and if wanted transform to base_link + try { + geometry_msgs::msg::TransformStamped + current_support_to_base_msg = + tf_buffer_->lookupTransform(previous_support_link_, base_link_frame_, rclcpp::Time(0, 0, RCL_ROS_TIME)); + tf2::Transform current_support_to_base; + tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); + double x = current_support_to_base.getOrigin().x(); + if (current_odom_msg_.twist.twist.linear.x > 0) { + x = x * config_.x_forward_scaling; + } else { + x = x * config_.x_backward_scaling; + } + double y = current_support_to_base.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; + current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + current_support_to_base.setRotation(q); + + tf2::Transform odom_to_base_link = odometry_to_support_foot_ * current_support_to_base; + geometry_msgs::msg::TransformStamped odom_to_base_link_msg = geometry_msgs::msg::TransformStamped(); + odom_to_base_link_msg.transform = tf2::toMsg(odom_to_base_link); + odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; + odom_to_base_link_msg.header.frame_id = odom_frame_; + odom_to_base_link_msg.child_frame_id = base_link_frame_; + if (config_.publish_walk_odom_tf) { + RCLCPP_WARN_ONCE(this->get_logger(), "Sending Tf from walk odometry directly"); + br_->sendTransform(odom_to_base_link_msg); + } + + // odometry as message + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = current_support_to_base_msg.header.stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.child_frame_id = base_link_frame_; + odom_msg.pose.pose.position.x = odom_to_base_link_msg.transform.translation.x; + odom_msg.pose.pose.position.y = odom_to_base_link_msg.transform.translation.y; + odom_msg.pose.pose.position.z = odom_to_base_link_msg.transform.translation.z; + odom_msg.pose.pose.orientation = odom_to_base_link_msg.transform.rotation; + odom_msg.twist = current_odom_msg_.twist; + pub_odometry_->publish(odom_msg); + + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); + return; + } + } void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg) { @@ -205,11 +202,6 @@ void MotionOdometry::supportCallback(const biped_interfaces::msg::Phase::SharedP } } -void MotionOdometry::jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg) { - current_joint_states_ = *msg; - joint_update_time_ = msg->header.stamp; -} - void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { current_odom_msg_ = *msg; } @@ -221,10 +213,11 @@ int main(int argc, char **argv) { auto node = std::make_shared(); rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); - rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); rclcpp::experimental::executors::EventsExecutor exec; exec.add_node(node); + rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); + exec.spin(); rclcpp::shutdown(); } diff --git a/bitbots_odometry/src/odometry_fuser.cpp b/bitbots_odometry/src/odometry_fuser.cpp index 3411af15c..23a8180ce 100644 --- a/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_odometry/src/odometry_fuser.cpp @@ -10,7 +10,7 @@ imu (rX, rY) // TODO Doku OdometryFuser::OdometryFuser() : Node("OdometryFuser"), - tf_buffer_(std::make_unique(this->get_clock())), + tf_buffer_(std::make_shared(this->get_clock())), tf_listener_(std::make_shared(*tf_buffer_, this)), support_state_cache_(100), imu_sub_(this, "imu/data"), @@ -28,8 +28,6 @@ OdometryFuser::OdometryFuser() : Node("OdometryFuser"), this->get_parameter("odom_frame", odom_frame_); this->declare_parameter("rotation_frame", "rotation"); this->get_parameter("rotation_frame", rotation_frame_); - this->declare_parameter("imu_frame", "imu_frame"); - this->get_parameter("imu_frame", imu_frame_); walk_support_state_sub_ = this->create_subscription("walk_support_state", @@ -45,67 +43,64 @@ OdometryFuser::OdometryFuser() : Node("OdometryFuser"), fused_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); } -bool OdometryFuser::wait_for_tf() { - // wait for transforms from joints - if (!tf_buffer_->canTransform(l_sole_frame_, - base_link_frame_, - rclcpp::Time(0, 0, RCL_ROS_TIME), - rclcpp::Duration::from_nanoseconds(1*1e9)) - && rclcpp::ok()) { - // don't spam directly with warnings, since it is normal that it will take a second to get the transform - if ((this->now() - start_time_).seconds() > 10) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30000, "Waiting for transforms from robot joints"); - } - return false; - } - return true; -} - void OdometryFuser::loop() { - // get roll an pitch from imu - tf2::Quaternion imu_orientation; - tf2::fromMsg(imu_data_.orientation, imu_orientation); + bitbots_utils::wait_for_tf( + this->get_logger(), + this->get_clock(), + this->tf_buffer_, + {base_link_frame_, r_sole_frame_, l_sole_frame_}, + base_link_frame_); // get motion_odom transform tf2::Transform motion_odometry; tf2::fromMsg(odom_data_.pose.pose, motion_odometry); // combine orientations to new quaternion if IMU is active, use purely odom otherwise - tf2::Transform fused_odometry; - - // compute the point of rotation (in base_link frame) - tf2::Transform rotation_point_in_base = getCurrentRotationPoint(); - // get base_link in rotation point frame - tf2::Transform base_link_in_rotation_point = rotation_point_in_base.inverse(); - - // get only translation and yaw from motion odometry - tf2::Quaternion odom_orientation_yaw = getCurrentMotionOdomYaw( - motion_odometry.getRotation()); - tf2::Transform motion_odometry_yaw; - motion_odometry_yaw.setRotation(odom_orientation_yaw); - motion_odometry_yaw.setOrigin(motion_odometry.getOrigin()); - - // Get the rotation offset between the IMU and the baselink - tf2::Transform imu_mounting_offset; - try { - geometry_msgs::msg::TransformStamped imu_mounting_transform = tf_buffer_->lookupTransform( - base_link_frame_, imu_frame_, fused_time_); - fromMsg(imu_mounting_transform.transform, imu_mounting_offset); - } catch (tf2::TransformException &ex) { - RCLCPP_ERROR(this->get_logger(), "Not able to use the IMU%s", ex.what()); - } + tf2::Transform fused_odometry {motion_odometry}; + + // Check if the imu is active + if (imu_data_received_) { + + // get roll an pitch from imu + tf2::Quaternion imu_orientation; + tf2::fromMsg(imu_data_.orientation, imu_orientation); + + // compute the point of rotation (in base_link frame) + tf2::Transform rotation_point_in_base = getCurrentRotationPoint(); + // get base_link in rotation point frame + tf2::Transform base_link_in_rotation_point = rotation_point_in_base.inverse(); - // get imu transform without yaw - tf2::Quaternion imu_orientation_without_yaw_component = getCurrentImuRotationWithoutYaw( - imu_orientation * imu_mounting_offset.getRotation()); - tf2::Transform imu_without_yaw_component; - imu_without_yaw_component.setRotation(imu_orientation_without_yaw_component); - imu_without_yaw_component.setOrigin({0, 0, 0}); + // get only translation and yaw from motion odometry + tf2::Quaternion odom_orientation_yaw = getCurrentMotionOdomYaw( + motion_odometry.getRotation()); + tf2::Transform motion_odometry_yaw; + motion_odometry_yaw.setRotation(odom_orientation_yaw); + motion_odometry_yaw.setOrigin(motion_odometry.getOrigin()); - // transformation chain to get correctly rotated odom frame - // go to the rotation point in the odom frame. rotate the transform to the base link at this point - fused_odometry = - motion_odometry_yaw * rotation_point_in_base * imu_without_yaw_component * base_link_in_rotation_point; + // Get the rotation offset between the IMU and the baselink + tf2::Transform imu_mounting_offset; + try { + geometry_msgs::msg::TransformStamped imu_mounting_transform = tf_buffer_->lookupTransform( + imu_data_.header.frame_id, base_link_frame_, fused_time_); + fromMsg(imu_mounting_transform.transform, imu_mounting_offset); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Not able to fuse IMU data with odometry due to a tf problem: %s", ex.what()); + } + + // get imu transform without yaw + tf2::Quaternion imu_orientation_without_yaw_component = getCurrentImuRotationWithoutYaw( + imu_orientation * imu_mounting_offset.getRotation()); + tf2::Transform imu_without_yaw_component; + imu_without_yaw_component.setRotation(imu_orientation_without_yaw_component); + imu_without_yaw_component.setOrigin({0, 0, 0}); + + // transformation chain to get correctly rotated odom frame + // go to the rotation point in the odom frame. rotate the transform to the base link at this point + fused_odometry = + motion_odometry_yaw * rotation_point_in_base * imu_without_yaw_component * base_link_in_rotation_point; + } else { + fused_odometry = motion_odometry; + } // combine it all into a tf tf_.header.stamp = fused_time_; @@ -244,6 +239,7 @@ void OdometryFuser::imuCallback( // Use the time of the imu as a baseline to do transforms and stuff because it is more timecritical than the walking odometry. // The walking odom stamp is also close to this timestamp due to the Synchronizer policy. fused_time_ = imu_data_.header.stamp; + imu_data_received_ = true; } int main(int argc, char **argv) { @@ -251,9 +247,7 @@ int main(int argc, char **argv) { auto node = std::make_shared(); rclcpp::experimental::executors::EventsExecutor exec; exec.add_node(node); - while(!node->wait_for_tf()){ - exec.spin_some(); - } + rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 100.0); rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); diff --git a/bitbots_quintic_walk/CMakeLists.txt b/bitbots_quintic_walk/CMakeLists.txt index f4bddc506..fb0e87230 100644 --- a/bitbots_quintic_walk/CMakeLists.txt +++ b/bitbots_quintic_walk/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(control_msgs REQUIRED) find_package(control_toolbox REQUIRED) find_package(generate_parameter_library REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(humanoid_league_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) @@ -70,7 +69,6 @@ ament_target_dependencies(WalkNode control_msgs control_toolbox geometry_msgs - humanoid_league_msgs moveit_ros_planning_interface nav_msgs rclcpp @@ -99,7 +97,6 @@ ament_target_dependencies(libpy_quintic_walk PUBLIC control_msgs control_toolbox geometry_msgs - humanoid_league_msgs moveit_ros_planning_interface nav_msgs rclcpp diff --git a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h index f9bdca41b..02d6a28ae 100644 --- a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h +++ b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h @@ -27,10 +27,10 @@ The original files can be found at: #include #include #include -#include "humanoid_league_msgs/msg/robot_control_state.hpp" -#include "bitbots_msgs/msg/joint_command.hpp" -#include "bitbots_msgs/msg/foot_pressure.hpp" #include "biped_interfaces/msg/phase.hpp" +#include "bitbots_msgs/msg/foot_pressure.hpp" +#include "bitbots_msgs/msg/joint_command.hpp" +#include "bitbots_msgs/msg/robot_control_state.hpp" #include #include @@ -102,7 +102,7 @@ class WalkNode : public rclcpp::Node { * Sets the current state of the robot * @param msg The current state */ - void robotStateCb(humanoid_league_msgs::msg::RobotControlState::SharedPtr msg); + void robotStateCb(bitbots_msgs::msg::RobotControlState::SharedPtr msg); WalkEngine *getEngine(); WalkIK *getIk(); @@ -175,7 +175,7 @@ class WalkNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr step_sub_; rclcpp::Subscription::SharedPtr cmd_vel_sub_; - rclcpp::Subscription::SharedPtr robot_state_sub_; + rclcpp::Subscription::SharedPtr robot_state_sub_; rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr kick_sub_; rclcpp::Subscription::SharedPtr imu_sub_; diff --git a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h index 4f4996622..1eece0a40 100644 --- a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h +++ b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h @@ -5,16 +5,16 @@ #include #include #include "bitbots_quintic_walk/walk_utils.h" -#include -#include #include #include -#include +#include +#include +#include +#include +#include #include #include -#include #include -#include namespace py = pybind11; using namespace ros2_python_extension; diff --git a/bitbots_quintic_walk/launch/test.launch b/bitbots_quintic_walk/launch/test.launch index 297a93ac0..868568311 100644 --- a/bitbots_quintic_walk/launch/test.launch +++ b/bitbots_quintic_walk/launch/test.launch @@ -7,9 +7,24 @@ - + - + + + + + + + + + + + + + + + + diff --git a/bitbots_quintic_walk/package.xml b/bitbots_quintic_walk/package.xml index ce1f7a3b1..e91791448 100644 --- a/bitbots_quintic_walk/package.xml +++ b/bitbots_quintic_walk/package.xml @@ -27,12 +27,12 @@ bio_ik bitbots_docs bitbots_msgs + bitbots_robot_description bitbots_splines bitbots_utils control_toolbox generate_parameter_library geometry_msgs - humanoid_league_msgs moveit_core moveit_ros_move_group moveit_ros_planning_interface diff --git a/bitbots_quintic_walk/src/walk_engine.cpp b/bitbots_quintic_walk/src/walk_engine.cpp index c7ee603d3..bbc02edbb 100644 --- a/bitbots_quintic_walk/src/walk_engine.cpp +++ b/bitbots_quintic_walk/src/walk_engine.cpp @@ -365,7 +365,7 @@ void WalkEngine::reset(WalkState state, buildStartStepTrajectories(); } else if (state == WalkState::STOP_MOVEMENT) { buildStopMovementTrajectories(); - } else if (state == WalkState::START_STEP) { + } else if (state == WalkState::STOP_STEP) { buildStopStepTrajectories(); } else if (state == WalkState::KICK) { buildKickTrajectories(); @@ -399,7 +399,7 @@ void WalkEngine::reset(WalkState state, buildStartStepTrajectories(); } else if (state == WalkState::STOP_MOVEMENT) { buildStopMovementTrajectories(); - } else if (state == WalkState::START_STEP) { + } else if (state == WalkState::STOP_STEP) { buildStopStepTrajectories(); } else if (state == WalkState::KICK) { buildKickTrajectories(); @@ -1114,4 +1114,4 @@ tf2::Transform WalkEngine::getLeft() { tf2::Transform WalkEngine::getRight() { return right_in_world_; } -} // namespace bitbots_quintic_walk \ No newline at end of file +} // namespace bitbots_quintic_walk diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index 8b27651ba..2b80c3054 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -74,7 +74,7 @@ WalkNode::WalkNode(const std::string ns, std::vector paramete this->get_parameter("odom_frame", odom_frame_); // init variables - robot_state_ = humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE; + robot_state_ = bitbots_msgs::msg::RobotControlState::CONTROLLABLE; current_request_.linear_orders = {0, 0, 0}; current_request_.angular_z = 0; current_request_.stop_walk = true; @@ -104,7 +104,7 @@ WalkNode::WalkNode(const std::string ns, std::vector paramete "cmd_vel", 1, std::bind(&WalkNode::cmdVelCb, this, _1)); - robot_state_sub_ = this->create_subscription( + robot_state_sub_ = this->create_subscription( "robot_state", 1, std::bind(&WalkNode::robotStateCb, this, _1)); @@ -164,8 +164,8 @@ void WalkNode::run() { double dt = getTimeDelta(); // necessary as timer in simulation does not work correctly https://github.com/ros2/rclcpp/issues/465 if (dt != 0.0) { - if (robot_state_==humanoid_league_msgs::msg::RobotControlState::FALLING - || robot_state_==humanoid_league_msgs::msg::RobotControlState::GETTING_UP) { + if (robot_state_==bitbots_msgs::msg::RobotControlState::FALLING + || robot_state_==bitbots_msgs::msg::RobotControlState::GETTING_UP) { // the robot fell, we have to reset everything and do nothing else walk_engine_.reset(); stabilizer_.reset(); @@ -174,9 +174,9 @@ void WalkNode::run() { /* Our robots will soon^TM be able to sit down and stand up autonomously, when sitting down the motors are * off but will turn on automatically which is why MOTOR_OFF is a valid walkable state. */ // TODO Figure out a better way than having integration knowledge that HCM will play an animation to stand up - current_request_.walkable_state = robot_state_==humanoid_league_msgs::msg::RobotControlState::CONTROLLABLE || - robot_state_==humanoid_league_msgs::msg::RobotControlState::WALKING || - robot_state_==humanoid_league_msgs::msg::RobotControlState::MOTOR_OFF; + current_request_.walkable_state = robot_state_==bitbots_msgs::msg::RobotControlState::CONTROLLABLE || + robot_state_==bitbots_msgs::msg::RobotControlState::WALKING || + robot_state_==bitbots_msgs::msg::RobotControlState::MOTOR_OFF; // reset when we start walking, otherwise PID controller will use old I value if ((last_request_.linear_orders[0]==0 && last_request_.linear_orders[1]==0 && last_request_.angular_z==0) @@ -534,7 +534,7 @@ void WalkNode::checkPhaseRestAndReset() { } } -void WalkNode::robotStateCb(const humanoid_league_msgs::msg::RobotControlState::SharedPtr msg) { +void WalkNode::robotStateCb(const bitbots_msgs::msg::RobotControlState::SharedPtr msg) { robot_state_ = msg->state; } diff --git a/bitbots_quintic_walk/src/walk_pywrapper.cpp b/bitbots_quintic_walk/src/walk_pywrapper.cpp index ea6e9b2cd..79cbce560 100644 --- a/bitbots_quintic_walk/src/walk_pywrapper.cpp +++ b/bitbots_quintic_walk/src/walk_pywrapper.cpp @@ -147,9 +147,9 @@ bool PyWalkWrapper::is_left_support(){ } void PyWalkWrapper::set_robot_state(int state) { - humanoid_league_msgs::msg::RobotControlState state_msg; + bitbots_msgs::msg::RobotControlState state_msg; state_msg.state = state; - walk_node_->robotStateCb(std::make_shared(state_msg)); + walk_node_->robotStateCb(std::make_shared(state_msg)); } void PyWalkWrapper::set_parameter(py::bytes parameter_msg) { diff --git a/bitbots_quintic_walk/test/rostests/test_walk.launch b/bitbots_quintic_walk/test/rostests/test_walk.launch deleted file mode 100644 index 580a82750..000000000 --- a/bitbots_quintic_walk/test/rostests/test_walk.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/bitbots_quintic_walk/test/rostests/test_walk.py b/bitbots_quintic_walk/test/rostests/test_walk.py deleted file mode 100755 index ec49903cb..000000000 --- a/bitbots_quintic_walk/test/rostests/test_walk.py +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env python3 -import math -import time - -import rospy -from biped_interfaces.msg import Phase -from bitbots_msgs.msg import JointCommand -from geometry_msgs.msg import Twist, Point, Pose, Quaternion - -from bitbots_test.mocks import MockSubscriber -from bitbots_test.test_case import WebotsTestCase -from nav_msgs.msg import Odometry - -walkready = JointCommand( - joint_names=["HeadPan", "HeadTilt", "LShoulderPitch", "LShoulderRoll", "LElbow", "RShoulderPitch", - "RShoulderRoll", "RElbow"], - velocities=[5.0] * 8, - accelerations=[-1.0] * 8, - max_currents=[-1.0] * 8, - positions=[ - 0.0, # HeadPan - 0.0, # HeadTilt - math.radians(75.27), # LShoulderPitch - 0.0, # LShoulderRoll - math.radians(35.86), # LElbow - math.radians(-75.58), # RShoulderPitch - 0.0, # RShoulderRoll - math.radians(-36.10), # RElbow - ] -) - - -class TestWalk(WebotsTestCase): - - def initialize_everything(self): - # get arms walkready - joint_pub = rospy.Publisher("DynamixelController/command", JointCommand, queue_size=1) - joint_pub.publish(walkready) - - # start and stop walking to get walkready - pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) - cmd_vel = Twist() - cmd_vel.linear.x = 0.1 - pub.publish(cmd_vel) - rospy.sleep(1) - cmd_vel.linear.x = 0.0 - cmd_vel.angular.x = -1 - pub.publish(cmd_vel) - rospy.sleep(1) - - # remove ball - self.set_ball_position(Point(10, 0, 0)) - - # reset robot to start - self.set_robot_pose(Pose(Point(0, 0, 0.42), Quaternion(0, 0, 0, 1))) - - def test_start(self): - """ test if node starts correctly without warnings/errors/criticals - and if it is still there after some time (did not crash by itself)""" - # setup - self.initialize_everything() - # wait to make sure node is up - sub = MockSubscriber("DynamixelController/command", JointCommand, tcp_nodelay=True) - sub.wait_until_connected() - - # execution - time.sleep(2) - - # verification - self.assertNoNegativeRosLogs(node="walking") - - def test_no_joint_goals(self): - """test if joint goals are published when walking is activated and only then""" - # setup - self.initialize_everything() - sub = MockSubscriber("DynamixelController/command", JointCommand, tcp_nodelay=True) - sub.wait_until_connected() - - # execution - # wait some time - time.sleep(1) - - # verification - sub.assertNothingReceived() - - def test_joint_goals(self): - # setup - self.initialize_everything() - pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) - sub = MockSubscriber("DynamixelController/command", JointCommand, tcp_nodelay=True) - sub.wait_until_connected() - # wait for support state to make sure the walking is started - sub_support_state = MockSubscriber("walk_support_state", Phase, tcp_nodelay=True) - sub_support_state.wait_until_connected() - time.sleep(10) - - # execution - cmd_vel = Twist() - cmd_vel.linear.x = 0.1 - pub.publish(cmd_vel) - rospy.sleep(5) - - # verification - # make sure something is published - sub.assertMessageReceived() - - def test_walk(self): - """test if the walking is really moving the robot around in the simulation""" - self.initialize_everything() - # setup - pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) - - # execution - cmd_vel = Twist() - cmd_vel.linear.x = 0.1 - pub.publish(cmd_vel) - rospy.sleep(10) - cmd_vel = Twist() - pub.publish(cmd_vel) - rospy.sleep(2) - - # verification - # should have moved away - self.assertRobotNotPosition(Point(0, 0, 0), threshold=0.5) - # but still standing - self.assertRobotStanding() - - def test_walk_odometry(self): - """ - Test if the walk odometry is correct. - This means also that the robot is walking with the correct speed. - """ - - def odom_cb(msg): - nonlocal current_odom - current_odom = msg - - # setup - self.initialize_everything() - current_odom = None - sub = MockSubscriber("walk_engine_odometry", Odometry, odom_cb, tcp_nodelay=True) - sub.wait_until_connected() - pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) - # remember start odom pose - rospy.sleep(1) - start_odom = current_odom - - # execution - cmd_vel = Twist() - cmd_vel.linear.x = 0.1 - cmd_vel.linear.y = 0.05 - cmd_vel.angular.z = 0.1 - pub.publish(cmd_vel) - rospy.sleep(10) - cmd_vel = Twist() - cmd_vel.angular.x = -1 - pub.publish(cmd_vel) - rospy.sleep(2) - - # verification - end_odom = current_odom - odom_diff = Pose() - odom_diff.position.x = end_odom.pose.pose.position.x - start_odom.pose.pose.position.x - odom_diff.position.y = end_odom.pose.pose.position.y - start_odom.pose.pose.position.y - odom_diff.position.z = end_odom.pose.pose.position.z - start_odom.pose.pose.position.z - # difference between quaternions is annoying, so just use end pose, other tests did not turn - odom_diff.orientation.w = end_odom.pose.pose.orientation.w - odom_diff.orientation.x = end_odom.pose.pose.orientation.x - odom_diff.orientation.y = end_odom.pose.pose.orientation.y - odom_diff.orientation.z = end_odom.pose.pose.orientation.z - # robot should be at odom position - self.assertRobotPose(odom_diff, lin_threshold=1, ang_threshold=1) - self.assertRobotStanding() - - -if __name__ == "__main__": - from bitbots_test import run_rostests - - run_rostests(TestWalk) diff --git a/bitbots_quintic_walk/test/rostests/test_walk_runs.launch b/bitbots_quintic_walk/test/rostests/test_walk_runs.launch deleted file mode 100644 index 95f304849..000000000 --- a/bitbots_quintic_walk/test/rostests/test_walk_runs.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/bitbots_quintic_walk/test/rostests/test_walk_runs.py b/bitbots_quintic_walk/test/rostests/test_walk_runs.py deleted file mode 100755 index 65b708da4..000000000 --- a/bitbots_quintic_walk/test/rostests/test_walk_runs.py +++ /dev/null @@ -1,24 +0,0 @@ -#!/usr/bin/env python3 -from bitbots_test.test_case import RosNodeTestCase -from bitbots_test.mocks import MockSubscriber -from geometry_msgs.msg import Twist -from bitbots_msgs.msg import JointCommand -import rospy - - -class WalkRunsTestCase(RosNodeTestCase): - def test_walk_runs(self): - sub = MockSubscriber('walking_motor_goals', JointCommand) - self.with_assertion_grace_period(lambda: self.assertNodeRunning("walking"), 10) - pub = rospy.Publisher('cmd_vel', Twist, queue_size=1, latch=True) - - goal = Twist() - goal.linear.x = 0.3 - pub.publish(goal) - sub.wait_until_connected() - rospy.sleep(5) - self.with_assertion_grace_period(sub.assertMessageReceived, 100) - -if __name__ == '__main__': - from bitbots_test import run_rostests - run_rostests(WalkRunsTestCase) diff --git a/bitbots_recordui_rqt/AMENT_IGNORE b/bitbots_recordui_rqt/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/bitbots_recordui_rqt/CMakeLists.txt b/bitbots_recordui_rqt/CMakeLists.txt deleted file mode 100644 index 6b3990d2d..000000000 --- a/bitbots_recordui_rqt/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(bitbots_recordui_rqt) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED bitbots_docs) -catkin_package() -catkin_python_setup() - -enable_bitbots_docs() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/bitbots_recordui_rqt/docs/_static/logo.png b/bitbots_recordui_rqt/docs/_static/logo.png deleted file mode 100644 index f8afdd5d0..000000000 Binary files a/bitbots_recordui_rqt/docs/_static/logo.png and /dev/null differ diff --git a/bitbots_recordui_rqt/docs/conf.py b/bitbots_recordui_rqt/docs/conf.py deleted file mode 100644 index 586c7734e..000000000 --- a/bitbots_recordui_rqt/docs/conf.py +++ /dev/null @@ -1,193 +0,0 @@ -# -*- coding: utf-8 -*- -# -# Full list of options at http://www.sphinx-doc.org/en/master/config - -# -- Path setup -------------------------------------------------------------- - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -# -import os -import sys -import catkin_pkg.package - -from exhale import utils - -package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) -sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) - - -# -- Helper functions -------------------------------------------------------- - -def count_files(): - """:returns tuple of (num_py, num_cpp)""" - num_py = 0 - num_cpp = 0 - - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): - for f in files: - if f.endswith(".py"): - num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): - for f in files: - if f.endswith(".h") or f.endswith(".hpp"): - num_cpp += 1 - - return num_py, num_cpp - - -# -- Project information ----------------------------------------------------- - -project = catkin_package.name -copyright = '2019, Bit-Bots' -author = ", ".join([a.name for a in catkin_package.authors]) - -# The short X.Y version -version = str(catkin_package.version) -# The full version, including alpha/beta/rc tags -release = str(catkin_package.version) - -# -- General configuration --------------------------------------------------- - -# If your documentation needs a minimal Sphinx version, state it here. -# -# needs_sphinx = '1.0' - -# Add any Sphinx extension module names here, as strings. They can be -# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom -# ones. -extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', -] - -# Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] - -# The suffix(es) of source filenames. -# You can specify multiple suffix as a list of string: -# -# source_suffix = ['.rst', '.md'] -source_suffix = '.rst' - -# The master toctree document. -master_doc = 'index' - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -# -# This is also used if you do content translation via gettext catalogs. -# Usually you set "language" from the command line for these cases. -language = None - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -# This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = None - -# -- Exhale and Breath setup ------------------------------------------------- - -# Tell sphinx what the primary language being documented is. -num_files_py, num_files_cpp = count_files() -primary_domain = "py" if num_files_py >= num_files_cpp else "cpp" - -# Tell sphinx what the pygments highlight language should be. -highlight_language = primary_domain - -if num_files_cpp > 0: - extensions += [ - 'breathe', - 'exhale', - ] - - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } - breathe_default_project = project - - def specifications_for_kind(kind): - # Show all members for classes and structs - if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] - # An empty list signals to Exhale to use the defaults - else: - return [] - - exhale_args = { - # These arguments are required - "containmentFolder": "cppapi", - "rootFileName": "library_root.rst", - "rootFileTitle": "C++ Library API", - "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), - # Suggested optional arguments - "createTreeView": True, - "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) - } - -# -- Options for HTML output ------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -html_theme = 'sphinx_rtd_theme' - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# -# html_theme_options = {} - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] - -# Custom sidebar templates, must be a dictionary that maps document names -# to template names. -# -# The default sidebars (for documents that don't match any pattern) are -# defined by theme itself. Builtin themes are using these templates by -# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', -# 'searchbox.html']``. -# -# html_sidebars = {} - -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') - - -# -- Options for intersphinx extension --------------------------------------- - -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} - -# -- Options for todo extension ---------------------------------------------- - -# If true, `todo` and `todoList` produce output, else they produce nothing. -todo_include_todos = True - -# -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) -rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") diff --git a/bitbots_recordui_rqt/docs/index.rst b/bitbots_recordui_rqt/docs/index.rst deleted file mode 100644 index e76aa433a..000000000 --- a/bitbots_recordui_rqt/docs/index.rst +++ /dev/null @@ -1,21 +0,0 @@ -Welcome to |project|'s documentation! -================================================ - -Description ------------ - -|description| - -.. toctree:: - :maxdepth: 2 - - cppapi/library_root - pyapi/modules - - -Indices and tables -================== - -* :ref:`genindex` -* |modindex| -* :ref:`search` diff --git a/bitbots_recordui_rqt/init.bag b/bitbots_recordui_rqt/init.bag deleted file mode 100644 index 69929cfc1..000000000 Binary files a/bitbots_recordui_rqt/init.bag and /dev/null differ diff --git a/bitbots_recordui_rqt/package.xml b/bitbots_recordui_rqt/package.xml deleted file mode 100644 index b2cc1151f..000000000 --- a/bitbots_recordui_rqt/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - bitbots_recordui_rqt - 1.0.0 - TODO - - - Sebastian Stelter - Hamburg Bit-Bots - - Jasper Güldenstein - - MIT - - catkin - - python_qt_binding - python3-rospkg - qt_dotgraph - rosgraph - rosgraph_msgs - roslib - rosnode - rospy - rosservice - rostopic - rqt_gui - rqt_gui_py - bitbots_hcm - - - - - - tested_robot - python3 - - - - diff --git a/bitbots_recordui_rqt/plugin.xml b/bitbots_recordui_rqt/plugin.xml deleted file mode 100644 index a7e1e35b4..000000000 --- a/bitbots_recordui_rqt/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - TODO - - - - - folder - Plugins related to RoboCup. - - - preferences-system-network - TODO - - - diff --git a/bitbots_recordui_rqt/resource/RecordUI.ui b/bitbots_recordui_rqt/resource/RecordUI.ui deleted file mode 100644 index dd5d6ca4e..000000000 --- a/bitbots_recordui_rqt/resource/RecordUI.ui +++ /dev/null @@ -1,339 +0,0 @@ - - - MainWindow - - - - 0 - 0 - 1285 - 670 - - - - MainWindow - - - - - - - QLayout::SetNoConstraint - - - - - - - - - - - - Author - - - - - - - - 16777215 - 150 - - - - - - - - Animation Name - - - - - - - - - - Version - - - - - - - - - - Description - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - - - - - - - - - - - - Frame Name - - - - - - - - - - Duration - - - - - - - 0.100000000000000 - - - 1.000000000000000 - - - - - - - Pause - - - - - - - 0.100000000000000 - - - - - - - Record - - - - - - - - - - - - - - - - - Power mode - - - - - Save mode - - - - - - - - - 1 - - - - - - - - - - - - - toolBar - - - TopToolBarArea - - - false - - - - - - - - - - - - - - - - - - - - - - - New - - - Ctrl+N - - - - - Open - - - Ctrl+O - - - - - Save - - - Ctrl+S - - - - - Save as - - - Ctrl+Shift+S - - - - - Play Init - - - Ctrl+I - - - - - Play Frame - - - Ctrl+P - - - - - Play Next - - - Alt+P - - - - - Play Animation - - - Ctrl+Shift+P - - - - - Play Until - - - Ctrl+Alt+P - - - - - Duplicate Frame - - - + - - - - - Delete Frame - - - - - - - - - Undo - - - Ctrl+Z - - - - - Redo - - - Ctrl+Y - - - - - Mirror to Left - - - Ctrl+Left - - - - - Mirror to Right - - - Ctrl+Right - - - - - Invert - - - Ctrl+Down - - - - - Help - - - Ctrl+H - - - - - - diff --git a/bitbots_recordui_rqt/setup.py b/bitbots_recordui_rqt/setup.py deleted file mode 100644 index 1d654f18e..000000000 --- a/bitbots_recordui_rqt/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['bitbots_recordui_rqt'], - package_dir={'': 'src'}, - #scripts=['scripts/motion_viz'] -) - -setup(**d) diff --git a/bitbots_recordui_rqt/src/bitbots_recordui_rqt/__init__.py b/bitbots_recordui_rqt/src/bitbots_recordui_rqt/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/bitbots_recordui_rqt/src/bitbots_recordui_rqt/animation_recording.py b/bitbots_recordui_rqt/src/bitbots_recordui_rqt/animation_recording.py deleted file mode 100644 index cc7db92bb..000000000 --- a/bitbots_recordui_rqt/src/bitbots_recordui_rqt/animation_recording.py +++ /dev/null @@ -1,377 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -import datetime - -# todo -# copy paste of frames, from one animation to another -# record button next to frame name filed -# set min max for joint value fields - -import json -import os -import math - -import actionlib -import humanoid_league_msgs -import rosparam -from rosgraph import MasterException -import rospy -from copy import deepcopy -from socket import gethostname -import rospkg -from humanoid_league_msgs.msg import PlayAnimationAction - -import subprocess - - -class AnimationData(object): - ''' - Defines a current status of the recorded Animation - ''' - - def __init__(self): - self.anim_steps = [] - self.name = "None yet" - self.version = 0 - self.last_edited = datetime.datetime.isoformat(datetime.datetime.now(), ' ') - self.author = "Unknown" - self.last_hostname = "Unknown" - self.description = "Edit me!" - - -class Recorder(object): - ''' Recorder Methods are gathered in this class - - :param ipc: Shared Memory provider to set data - :param gui: urwid-gui responsible for displaying this reocrd-instance - :param logger: - the logger to use, defaults to 'record-gui' the logger is - important for the commuication with the gui-console - ''' - - def __init__(self): - self.steps = [] - self.redo_steps = [] - self.current_state = AnimationData() - self.anim_client = actionlib.SimpleActionClient('animation', PlayAnimationAction) - self.save_step('Initial step') - - def get_animation_state(self): - return self.current_state.anim_steps - - def get_meta_data(self): - data = self.current_state - return data.name, data.version, data.author, data.description - - def set_meta_data(self, name, version, author, description): - self.current_state.name = name - self.current_state.version = version - self.current_state.author = author - self.current_state.description = description - - def save_step(self, description, state=None): - ''' Save the current state of the Animation - for later restoration by the undo-command - - (Yes we might save only a diff, but the Memory consumption - should still be relatively low this way and saving / undoing - is really cheap in terms of CPU and effort spent programming) - - :param description: A string describing the saved action for the user - :param state: a AnimState can be given otherwise the current one is used - ''' - - rospy.logdebug("Saving step: %s" % description) - if not state: - state = deepcopy(self.current_state) - self.steps.append((state, description)) - self.save_animation("backup") - - def undo(self, amount=1): - ''' Undo of steps or the last Step if omitted - ''' - if amount > len(self.steps) or self.steps[-1][1] == "Initial step": - rospy.logwarn("I cannot undo what did not happen!") - return "I cannot undo what did not happen!" - if amount == 1: - state, description = self.steps.pop() - self.redo_steps.append((state, description, self.current_state)) - rospy.loginfo("Undoing: %s" % description) - if self.steps: - state, description = self.steps[-1] - self.current_state = state - rospy.loginfo("Last noted action: %s" % description) - return "Undoing. Last noted action: %s" % description - else: - rospy.loginfo("There are no previously noted steps") - return "Undoing. There are no more previous steps." - else: - rospy.loginfo("Undoing %i steps" % amount) - state, description = self.steps[-amount] - self.current_state = state - self.redo_steps = self.steps[-amount:].reverse() - self.steps = self.steps[:-amount] - return "Undoing %i steps" % amount - - def redo(self, amount=1): - ''' Redo of steps, or the last step if omitted - ''' - post_state = None - if not self.redo_steps: - rospy.logwarn("Cannot redo what was not undone!") - return "Cannot redo what was not undone!" - if amount < 0: - rospy.logwarn("Amount cannot be negative! (What where you even thinking?)") - return "Amount cannot be negative! (What where you even thinking?)" - while amount and self.redo_steps: - pre_state, description, post_state = self.redo_steps.pop() - self.steps.append((pre_state, description)) - amount -= 1 - self.current_state = post_state - rospy.loginfo("Last noted step is now: %s " % self.steps[-1][1]) - return "Last noted step is now: %s " % self.steps[-1][1] - - def record(self, motor_pos, motor_torque, frame_name, duration, pause, seq_pos=None, override=False): - ''' Record Command, save current keyframe-data - ''' - frame = { - "name": frame_name, - "duration": duration, - "pause": pause, - "goals": motor_pos, - "torque": motor_torque - } - new_frame = deepcopy(frame) - if seq_pos is None: - self.current_state.anim_steps.append(new_frame) - self.save_step("Appending new keyframe " + frame_name) - elif not override: - self.current_state.anim_steps.insert(seq_pos, new_frame) - self.save_step("Inserting new keyframe " + frame_name + " to position " + str(seq_pos)) - else: - self.current_state.anim_steps[seq_pos] = new_frame - self.save_step("overriding keyframe " + frame_name + " at position " + str(seq_pos)) - return True - - def clear(self): - ''' Record Command, clear all keyframe-data - ''' - newsteps = [] - for i in self.steps: - if i[1] == 'Initial step': - newsteps.append(i) - self.steps = deepcopy(newsteps) - self.current_state.anim_steps = [] - return True - - def save_animation(self, path, file_name=None, save_checkbox=None): - ''' Record Command, dump all keyframedata to an animation .json file - - :param file_name: what name the new file should receive - ''' - if not self.current_state.anim_steps: - rospy.loginfo("There is nothing to save.") - return "There is nothing to save." - - if not file_name: - file_name = self.current_state.name - - if not os.path.isdir(path): - path = os.path.expanduser('~') - path = os.path.join(path, file_name + '.json') - rospy.logdebug("Saving to '%s'" % path) - - savedKeyframes = deepcopy(self.current_state.anim_steps) - - for kf in savedKeyframes: - if not save_checkbox == None: - if kf["name"] in save_checkbox: - for k, v in save_checkbox[kf["name"]].items(): - if v == 0: - kf["goals"].pop(k) - - anim = { - "name": self.current_state.name, - "version": self.current_state.version, - "last_edited": datetime.datetime.isoformat(datetime.datetime.now(), ' '), - "author": self.current_state.author, - "description": self.current_state.description, - "keyframes": savedKeyframes, - "hostname": gethostname() - } - - for kf in anim['keyframes']: - for k, v in kf['goals'].items(): - kf['goals'][k] = int(math.degrees(v)) - - with open(path, "w") as fp: - json.dump(anim, fp, sort_keys=True, indent=4) - return ("Saving to '%s'" % path + ". Done.") - - def remove(self, framenumber=None): - ''' Record Command, remove the last keyframedata - - :param framenumber: The Number of frame to remove. default is last - ''' - if not framenumber: - if not self.current_state.anim_steps: - rospy.logwarn("Nothing to revert, framelist is empty!") - return False - self.save_step("Reverting the last Keyframe (#%i)" % len(self.current_state.anim_steps)) - self.current_state.anim_steps.pop() - return True - else: - try: - framenumber = int(framenumber) - except TypeError: - rospy.logwarn("Optional framenumber must be Integer! (got %s)" % framenumber) - return False - if len(self.current_state.anim_steps) < framenumber: - rospy.logwarn("Invalid framenumber: %i" % framenumber) - return False - self.save_step("Reverting keyframe #%i" % framenumber) - framenumber -= 1 # Frameindices in the GUI are starting with 1, not 0 - self.current_state.anim_steps.pop(framenumber) - return True - - def load_animation(self, path): - ''' Record command, load a animation '.json' file - - :param path: path of the animation to load - ''' - data = [] - with open(path) as fp: - try: - data = json.load(fp) - i = 0 - for kf in data['keyframes']: - if not 'name' in kf: - kf['name'] = 'frame'+str(i) - i += 1 - for kf in data['keyframes']: - for k, v in kf['goals'].items(): - kf['goals'][k] = math.radians(v) - except ValueError as e: - rospy.logerr("Animation %s is corrupt:\n %s" % - (path, e.message.partition('\n')[0])) - return ("Animation %s is corrupt:\n %s" % (path, e.message.partition('\n')[0])) - - - # Ensure Data retrieval was a success - if not data: - return False - - self.save_step("Loading of animation named %s" % path) - - self.current_state.anim_steps = data['keyframes'] - - def play(self, anim_path, until_frame=None): - ''' Record command, start playing an animation - - Can play a certain (named) animation or the current one by default. - Also can play only a part of an animation if end is defined - - :param until_frame: the frame until which the animation should be played - ''' - try: - if not self.current_state.anim_steps: - rospy.loginfo("Refusing to play, because nothing to play exists!") - return "Refusing to play, because nothing to play exists!" - if not until_frame: - # play complete animation - n = len(self.current_state.anim_steps) - else: - # play the given number if not higher than current steps - n = min(until_frame, len(self.current_state.anim_steps)) - - anim_dict = { - "name": "Record-play", - "keyframes": deepcopy(self.current_state.anim_steps[0:n]) - } - for kf in anim_dict['keyframes']: - for k, v in kf['goals'].items(): - kf['goals'][k] = int(math.degrees(v)) - - rospy.loginfo("playing %d frames..." % len(anim_dict['keyframes'])) - self.execute_play(anim_dict, anim_path) - return "playing %d frames..." % len(anim_dict['keyframes']) - except MasterException: - rospy.logwarn("There is no Robot! Can't play Animation!") - return "There is no Robot! Can't play Animation!" - - def execute_play(self, anim_dict, anim_path): - ''' We make a temporary copy of the animation and call the animation play action to play it''' - anim_package = rosparam.get_param("robot_type_name").lower() + "_animations" - rospack = rospkg.RosPack() - path = rospack.get_path(anim_package) - path = os.path.join(path, "record" + '.json') - - - with open(path, "w") as fp: - json.dump(anim_dict, fp, sort_keys=True, indent=4) - p = subprocess.Popen(["scp", path, anim_path]).wait() - self.play_animation("record") - - def play_animation(self, name): - '''Sends the animation to the animation server ''' - first_try = self.anim_client.wait_for_server( - rospy.Duration(rospy.get_param("hcm/anim_server_wait_time", 10))) - if not first_try: - rospy.logerr( - "Animation Action Server not running! Will now wait until server is accessible!") - self.anim_client.wait_for_server() - rospy.logwarn("Animation server now running, hcm will go on.") - goal = humanoid_league_msgs.msg.PlayAnimationGoal() - goal.animation = name - goal.hcm = True # force - self.anim_client.send_goal_and_wait(goal) - rospy.sleep(0.5) - - def change_frame_order(self, new_order): - ''' Changes the order of the frames given an array of frame names''' - new_ordered_frames = [] - for frame_name in new_order: - for frame in self.current_state.anim_steps: - if frame_name == frame["name"]: - new_ordered_frames.append(frame) - - self.current_state.anim_steps = new_ordered_frames - self.save_step("Reordered frames") - - def duplicate(self, frame_name): - ''' - Duplicates a frame - ''' - new_frames = [] - for frame in self.current_state.anim_steps: - new_frames.append(frame) - if frame_name == frame["name"]: - duplicate = deepcopy(frame) - newname = frame_name + "d" - x = True - n = 0 - for frame in self.current_state.anim_steps: - if newname == frame["name"]: - while(x): - x = False - for frame in self.current_state.anim_steps: - if newname + str(n) == frame["name"]: - n+=1 - x = True - newname = newname+str(n) - - duplicate["name"] = newname - new_frames.append(duplicate) - self.current_state.anim_steps = new_frames - self.save_step("Duplicated Frame " + frame_name) - - def delete(self, frame_name): - ''' - Deletes a frame - ''' - new_frames = [] - for frame in self.current_state.anim_steps: - if not frame_name == frame["name"]: - new_frames.append(frame) - self.current_state.anim_steps = new_frames - self.save_step("Duplicated Frame " + frame_name) diff --git a/bitbots_recordui_rqt/src/bitbots_recordui_rqt/record_ui.py b/bitbots_recordui_rqt/src/bitbots_recordui_rqt/record_ui.py deleted file mode 100644 index 83f79a765..000000000 --- a/bitbots_recordui_rqt/src/bitbots_recordui_rqt/record_ui.py +++ /dev/null @@ -1,944 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -import rospkg -import rospy -import time -import math -import inspect -from copy import deepcopy - -from python_qt_binding.QtCore import Qt, QMetaType, QDataStream, QVariant, pyqtSignal -from python_qt_binding import loadUi -from rqt_gui_py.plugin import Plugin -from python_qt_binding.QtWidgets import QMainWindow, QTreeWidget, QTreeWidgetItem,QListWidgetItem, \ - QSlider, QGroupBox, QVBoxLayout, QLabel, QLineEdit, QListWidget, QAbstractItemView, QFileDialog, QDoubleSpinBox, QMessageBox, \ - QInputDialog, QShortcut -from python_qt_binding.QtGui import QDoubleValidator, QKeySequence - -from bitbots_msgs.msg import JointCommand, JointTorque -from sensor_msgs.msg import JointState -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint - -import os - -from .animation_recording import Recorder - - - -class DragDropList(QListWidget): - ''' QListWidget with an event that is called when a drag and drop action was performed.''' - keyPressed = pyqtSignal() - - def __init__(self, parent, ui): - super(DragDropList, self).__init__(parent) - - self.ui = ui - self.setAcceptDrops(True) - - - def dropEvent(self, e): - super(DragDropList, self).dropEvent(e) - items = [] - for i in range(0, self.count()): - items.append(self.item(i).text()) - self.ui.change_frame_order(items) - - def keyPressEvent(self, event): - if event.key() == Qt.Key_Delete: - super(DragDropList, self).keyPressEvent(event) - self.keyPressed.emit() - elif event.key() == Qt.Key_Up and self.currentRow()-1 >= 0: - self.setCurrentRow(self.currentRow()-1) - elif event.key() == Qt.Key_Down and self.currentRow()+1 < self.count(): - self.setCurrentRow(self.currentRow()+1) - - - - -class RecordUI(Plugin): - ''' class containing the UI part of the framework''' - def __init__(self, context): - super(RecordUI, self).__init__(context) - - - self._widget = QMainWindow() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('bitbots_recordui_rqt'), 'resource', 'RecordUI.ui') - loadUi(ui_file, self._widget, {}) - - self._recorder = Recorder() - self._sliders = {} - self._textFields = {} - self._motorSwitched = {} - self._selected_frame = None - - self._currentGoals = {} # this is the data about the current unsaved frame - self._currentDuration = 1.0 - self._currentPause = 0.0 - self._currentName = "new frame" - - self._workingValues = {} # this is the data about the frame that is displayed - self._workingDuration = 1.0 - self._workingPause = 0.0 - self._workingName = self._currentName - - self._current = True - - self._saveDir = None - - self._robot_anim_path = None - - #save current frame when switching to other frames for reference - #working frame - - self._treeItems = {} - self._motorCheckBody = QTreeWidgetItem(self._widget.motorTree) - self._motorCheckLegs = QTreeWidgetItem(self._motorCheckBody) - self._motorCheckArms = QTreeWidgetItem(self._motorCheckBody) - self._motorCheckHead = QTreeWidgetItem(self._motorCheckBody) - self._motorCheckLArm = QTreeWidgetItem(self._motorCheckArms) - self._motorCheckRArm = QTreeWidgetItem(self._motorCheckArms) - self._motorCheckLLeg = QTreeWidgetItem(self._motorCheckLegs) - self._motorCheckRLeg = QTreeWidgetItem(self._motorCheckLegs) - - #saves configuration of the trees checkboxes for each of the tree modes. - self._checkBoxesSave = {} - self._checkBoxesPower = {} - self._previousTreeMode = 0 - - self.setObjectName('Record Animation') - - self._initial_joints = None - - self._widget.frameList = DragDropList(self._widget, self) - self._widget.verticalLayout_2.insertWidget(0, self._widget.frameList) - self._widget.frameList.setDragDropMode(QAbstractItemView.InternalMove) - - self.state_sub = rospy.Subscriber("joint_states", JointState, self.state_update, queue_size=1, tcp_nodelay=True) - self._joint_pub = rospy.Publisher("record_motor_goals", JointCommand, queue_size=1) - self.effort_pub = rospy.Publisher("ros_control/set_torque_individual", JointTorque, queue_size=1) - - self.ids = {"HeadPan": 0, - "HeadTilt": 1, - "LShoulderPitch": 2, - "LShoulderRoll": 3, - "LElbow": 4, - "RShoulderPitch": 5, - "RShoulderRoll": 6, - "RElbow": 7, - "LHipYaw": 8, - "LHipRoll": 9, - "LHipPitch": 10, - "LKnee": 11, - "LAnklePitch": 12, - "LAnkleRoll": 13, - "RHipYaw": 14, - "RHipRoll": 15, - "RHipPitch": 16, - "RKnee": 17, - "RAnklePitch": 18, - "RAnkleRoll": 19} - - self._initial_joints = JointState() - - self.update_time = rospy.Duration(0.1) - - for k,v in self.ids.items(): - rospy.loginfo(k) - self._initial_joints.name.append(k) - self._initial_joints.position.append(0) - - while not self._initial_joints: - if not rospy.is_shutdown(): - time.rospy.sleep(0.5) - rospy.logwarn("wait") - else: - return - - self.initialize() - - context.add_widget(self._widget) - self._widget.statusBar.showMessage("Initialization complete.") - - def initialize(self): - for i in range(0, len(self._initial_joints.name)): - self._currentGoals[self._initial_joints.name[i]] = self._initial_joints.position[i] - self._workingValues[self._initial_joints.name[i]] = self._initial_joints.position[i] - self._motorSwitched[self._initial_joints.name[i]] = True - - - host = os.environ['ROS_MASTER_URI'].split('/')[2].split(':')[0] - self._robot_anim_path = "bitbots@{}:~/wolfgang_ws/src/wolfgang_robot/wolfgang_animations/animations/motion/".format(host) - - rospy.loginfo("Set animation path to: "+str(self._robot_anim_path+"record.json")) - - initTorque = {} - for k, v in self._workingValues.items(): - initTorque[k] = 2 - - self._checkBoxesPower['#CURRENT_FRAME'] = initTorque - - self.motor_controller() - self.motor_switcher() - self.action_connect() - self.box_ticked() - self.frame_list() - self.update_frames() - self.set_sliders_and_text_fields(manual=True) - - def state_update(self, joint_states): - ''' - Callback method for /joint_states. Updates the sliders to the actual values of the motors when the robot moves. - ''' - if not self._initial_joints: - self._initial_joints = joint_states - time.rospy.sleep(1) - else: - for i in range(0, len(joint_states.name)): - if (not self._motorSwitched[joint_states.name[i]]): - self._workingValues[joint_states.name[i]] = joint_states.position[i] - - - rospy.sleep(self.update_time) - self.set_sliders_and_text_fields(manual=False) - - - def motor_controller(self): - ''' - Sets up the GUI in the middle of the Screen to control the motors. - Uses self._motorValues to determine which motors are present. - ''' - i = 0 - for k, v in sorted(self._currentGoals.items()): - group = QGroupBox() - slider = QSlider(Qt.Horizontal) - slider.setTickInterval(1) - slider.setMinimum(-181) - slider.setMaximum(181) - slider.sliderMoved.connect(self.slider_update) # This has to be a sliderMoved signal, since valueChanged is - # triggered by other sources than user action. - self._sliders[k] = slider - - textfield = QLineEdit() - textfield.setText('0') - textfield.textEdited.connect(self.textfield_update) - self._textFields[k] = textfield - - label = QLabel() - label.setText(k) - - layout = QVBoxLayout() - layout.addWidget(label) - layout.addWidget(self._sliders[k]) - layout.addWidget(self._textFields[k]) - group.setLayout(layout) - self._widget.motorControlLayout.addWidget(group, i / 5, i % 5) - i = i+1 - - def action_connect(self): - ''' - Connects the actions in the top bar to the corresponding functions, and sets their shortcuts - :return: - ''' - self._widget.actionNew.triggered.connect(self.new) - self._widget.actionOpen.triggered.connect(self.open) - self._widget.actionSave.triggered.connect(self.save) - self._widget.actionSave_as.triggered.connect(self.save_as) - self._widget.actionInit.triggered.connect(self.goto_init) - self._widget.actionCurrent_Frame.triggered.connect(self.goto_frame) - self._widget.actionNext_Frame.triggered.connect(self.goto_next) - self._widget.actionAnimation.triggered.connect(self.play) - self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until) - self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate) - self._widget.actionDelete_Frame.triggered.connect(self.delete) - self._widget.actionLeft.triggered.connect(lambda: self.mirrorFrame("L")) - self._widget.actionRight.triggered.connect(lambda: self.mirrorFrame("R")) - self._widget.actionInvert.triggered.connect(self.invertFrame) - self._widget.actionUndo.triggered.connect(self.undo) - self._widget.actionRedo.triggered.connect(self.redo) - self._widget.actionHelp.triggered.connect(self.help) - - self._widget.buttonRecord.clicked.connect(self.record) - self._widget.buttonRecord.shortcut = QShortcut(QKeySequence("Space"), self._widget) - self._widget.buttonRecord.shortcut.activated.connect(self.record) - - - self._widget.frameList.keyPressed.connect(self.delete) - - self._widget.treeModeSelector.currentIndexChanged.connect(self.treeModeChanged) - - def help(self): - ''' - Prints out the help dialogue - ''' - helpDialog = QMessageBox.about(self._widget, "About RecordUI", "This is RecordUI, a tool to record robot animations.\n \n Keyboard shortcuts: \n \n \ - New: Ctrl + N \n \ - Open: Ctrl + O \n \ - Save: Ctrl + S \n \ - Save as: Ctrl + Shift + S \n \n \ - Play Init: Ctrl + I \n \ - Play Frame: Ctrl + P \n \ - Play Next: Alt + P \n \ - Play Animation: Ctrl + Shift + P \n \ - Play Until: Ctrl + Alt + P \n \n \ - Record Frame: Space \n \ - Duplicate Frame: '+' \n \ - Delete Frame: '-' \n \ - Undo: Ctrl + Z \n \ - Redo: Ctrl + Y \n \ - Help: Ctrl + H \n \n \ - Mirror to Left: Ctrl + Left Arrow \n \ - Mirror to Right: Ctrl + Right Arrow \n \ - Invert: Ctrl + Down Arrow") - - def new(self): - ''' - Deletes all currently recorded frames - ''' - if len(self._widget.frameList) > 1: - message = "This will discard your current Animation. Continue?" - sure = QMessageBox.question(self._widget, 'Sure?', message, QMessageBox.Yes | QMessageBox.No) - if sure == QMessageBox.Yes: - self._recorder.clear() - self.update_frames() - - def save(self): - ''' - Saves all currently recorded frames into a json file - ''' - if self._recorder.get_animation_state(): - self.treeModeChanged(self._previousTreeMode) - self.set_metadata() - if not self._saveDir: - rp = rospkg.RosPack() - #QFileDialogue throws a gtk warning, that can not be suppressed or fixed. Should be ignored. - self._saveDir = QFileDialog.getExistingDirectory(directory=str(rp.get_path('wolfgang_animations')+"/animations")) - status = self._recorder.save_animation(self._saveDir, self._widget.lineAnimationName.text(), self._checkBoxesSave) - self._widget.statusBar.showMessage(status) - else: - self._widget.statusBar.showMessage("There is nothing to save!") - return - - def save_as(self): - ''' - Saves all currently recorded frames into a json file, which is saved at a user specified location - ''' - if self._recorder.get_animation_state(): - self.treeModeChanged(self._previousTreeMode) - rp = rospkg.RosPack() - self._saveDir = QFileDialog.getExistingDirectory(directory=str(rp.get_path('wolfgang_animations')+"/animations")) - self._recorder.save_animation(self._saveDir, self._widget.lineAnimationName.text(), self._checkBoxesSave) - else: - self._widget.statusBar.showMessage("There is nothing to save!") - return - - - def set_metadata(self): - status = self._recorder.set_meta_data(self._widget.lineAnimationName.text(), - self._widget.lineVersion.text(), - self._widget.lineAuthor.text(), - self._widget.fieldDescription.toPlainText()) - self._widget.statusBar.showMessage(status) - def open(self): - ''' - Deletes all current frames and instead loads an animation from a json file - ''' - if len(self._widget.frameList) > 1: - message = "This will discard your current Animation. Continue?" - sure = QMessageBox.question(self._widget, 'Sure?', message, QMessageBox.Yes | QMessageBox.No) - if sure == QMessageBox.No: - return - rp = rospkg.RosPack() - my_file = QFileDialog.getOpenFileName(directory=str(rp.get_path('wolfgang_animations')+"/animations"))[0] - if my_file: - status = self._recorder.load_animation(my_file) - if status == "": - status = "Load successful." - self._widget.statusBar.showMessage(status) - - animstate = self._recorder.get_animation_state() - for i in animstate: - try: - self._checkBoxesPower[i['name']] = i['torque'] - except KeyError: - self._checkBoxesPower[i['name']] = {} - for key in self.ids: - self._checkBoxesPower[i['name']][key] = 2 - - - - self.update_frames() - - metadata = self._recorder.get_meta_data() - - self._widget.lineAnimationName.setText(metadata[0]) - self._widget.lineAuthor.setText(metadata[2]) - self._widget.lineVersion.setText(str(metadata[1])) - self._widget.fieldDescription.setPlainText(metadata[3]) - - def play(self): - ''' - Plays the animation - ''' - status = self._recorder.play(self._robot_anim_path) - self._widget.statusBar.showMessage(status) - - def play_until(self): - ''' - Plays the animation up to a certain frame - ''' - steps = self._recorder.get_animation_state() - j = 0 - for i in range(0, len(steps)): - j += 1 - rospy.loginfo(steps[i]["name"]) - if steps[i]["name"] == self._selected_frame["name"]: - break - self._recorder.play(self._robot_anim_path, j) - - def goto_frame(self): - ''' - Plays one single frame - ''' - self.set_all_joints_stiff() - - pos_msg = JointCommand() - pos_msg.joint_names = [] - pos_msg.velocities = [1.0] * 20 - pos_msg.positions = [] - pos_msg.accelerations = [-1.0] * 20 - pos_msg.max_currents = [-1.0] * 20 - - - for k,v in self._workingValues.items(): - pos_msg.joint_names.append(k) - pos_msg.positions.append(v) - - torque_msg = JointTorque() - torque_msg.joint_names = [] - torque_msg.on = [] - - for k, v in self._checkBoxesPower[self._widget.frameList.currentItem().text()].items(): - torque_msg.joint_names.append(k) - torque_msg.on.append(v) - - self.effort_pub.publish(torque_msg) - self._joint_pub.publish(pos_msg) - - def goto_next(self): - if self._widget.frameList.currentRow() < self._widget.frameList.count() - 2: - self._widget.frameList.setCurrentRow(self._widget.frameList.currentRow()+1) - self.goto_frame() - - def goto_init(self): - ''' - Plays init frame, sets all motor values to 0 - ''' - self.set_all_joints_stiff() - - if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME": - for k, v in self._workingValues.items(): - self._workingValues[k] = 0.0 - for k, v in self._currentGoals.items(): - self._currentGoals[k] = 0.0 - self.set_sliders_and_text_fields(manual=False) - - pos_msg = JointCommand() - pos_msg.joint_names = self._initial_joints.name - pos_msg.velocities = [1.0] * len(self._initial_joints.name) - pos_msg.positions = [0.0] * len(self._initial_joints.name) - pos_msg.accelerations = [-1.0] * len(self._initial_joints.name) - pos_msg.max_currents = [-1.0] * len(self._initial_joints.name) - - self._joint_pub.publish(pos_msg) - - def set_all_joints_stiff(self): - ''' - Enables torque for all motors - ''' - if self._widget.frameList.currentItem().text() == '#CURRENT_FRAME': - for k, v in self._treeItems.items(): - v.setCheckState(0, Qt.Checked) - - torque_msg = JointTorque() - torque_msg.joint_names = [] - torque_msg.on = [] - - for k, v in sorted(self._treeItems.items()): - torque_msg.joint_names.append(k) - torque_msg.on.append(True) - - self.effort_pub.publish(torque_msg) - - def duplicate(self): - ''' - Creates a copy of the selected frame - ''' - try: - frame = self._widget.frameList.selectedItems()[0].text() - except: - return - if frame: - if not frame == "#CURRENT_FRAME": - self._recorder.duplicate(frame) - self._widget.statusBar.showMessage("Duplicated frame "+ frame) - else: - self._widget.statusBar.showMessage("Cannot duplicate current frame. Record first.") - self.update_frames() - - def delete(self): - ''' - Deletes a frame - ''' - try: - frame = self._widget.frameList.selectedItems()[0].text() - except: - return - if frame: - if not frame == "#CURRENT_FRAME": - self._recorder.delete(frame) - self._widget.statusBar.showMessage("Deleted frame "+ frame) - else: - self._widget.statusBar.showMessage("Cannot delete current frame.") - self.update_frames() - - def record(self, keep=False): - ''' - Records a frame - ''' - if not self._widget.frameList.currentItem() == None: - if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME": - x = True - n = 0 - for state in self._recorder.get_animation_state(): - if self._workingName == state["name"]: - while(x): - x = False - for state in self._recorder.get_animation_state(): - if self._workingName+str(n) == state["name"]: - n+=1 - x = True - self._workingName = self._workingName+str(n) - - self.set_sliders_and_text_fields(manual=True) - - self._checkBoxesPower[self._workingName] = {} - initTorque = {} - for k, v in self._workingValues.items(): - initTorque[k] = 2 - - self._checkBoxesPower[self._workingName] = initTorque - - self._recorder.record(self._workingValues, - initTorque, - self._widget.lineFrameName.text(), - self._widget.spinBoxDuration.value(), - self._widget.spinBoxPause.value()) - self._currentGoals = deepcopy(self._workingValues) - else: - current_row = self._widget.frameList.currentRow() - - self._recorder.record(self._workingValues, - self._checkBoxesPower[self._widget.frameList.currentItem().text()], - self._widget.lineFrameName.text(), - self._widget.spinBoxDuration.value(), - self._widget.spinBoxPause.value(), - current_row, - True) - - - self._widget.statusBar.showMessage("Recorded frame "+ self._workingName) - self.update_frames(keep) - - - def undo(self): - ''' - Undos the previous action - ''' - status = self._recorder.undo() - self._widget.statusBar.showMessage(status) - self.update_frames() - - def redo(self): - ''' - Redos an action - ''' - status = self._recorder.redo() - self._widget.statusBar.showMessage(status) - self.update_frames() - - def mirrorFrame(self, direction): - ''' - Copies all motor values from one side of the robot to the other. Inverts values, if necessary - ''' - opposite = "L" - if direction == "L": - opposite = "R" - for k, v in self._workingValues.items(): - if k[0] == opposite: - for k1, v1 in self._workingValues.items(): - if k1 == str(direction)+k[1:]: - self._workingValues[k1] = v * -1 - - boxmode = 0 - if self._widget.treeModeSelector.currentIndex() == 0: - boxmode = self._checkBoxesPower[self._widget.frameList.currentItem().text()] - elif self._widget.treeModeSelector.currentIndex() == 1: - boxmode = self._checkBoxesSave[self._widget.frameList.currentItem().text()] - - for k, v in boxmode.items(): - if k[0] == opposite: - for k1, v1 in boxmode.items(): - if k1 == str(direction)+k[1:]: - if boxmode[k] == 0: - boxmode[k1] = 0 - elif boxmode[k] ==2: - boxmode[k1] = 2 - - self.updateTreeConfig(self._widget.treeModeSelector.currentIndex()) - self.box_ticked() - self._widget.statusBar.showMessage("Mirrored frame to "+ direction) - - - def invertFrame(self): - ''' - Copies all values from the left side to the right and all values from the right side to the left. - Inverts values, if necessary - ''' - tempDict={} - for k, v in self._workingValues.items(): - if k[0] == "L": - tempDict["R"+k[1:]] = -v - elif k[0] == "R": - tempDict["L"+k[1:]] = -v - for k, v in tempDict.items(): - self._workingValues[k] = v - - boxmode = 0 - if self._widget.treeModeSelector.currentIndex() == 0: - boxmode = self._checkBoxesPower - elif self._widget.treeModeSelector.currentIndex() == 1: - boxmode = self._checkBoxesSave - - for k, v in boxmode[self._widget.frameList.currentItem().text()].items(): - - if k[0] == "L": - if v == 2: - tempDict["R"+k[1:]] = 2 - elif v == 0: - tempDict["R"+k[1:]] = 0 - elif k[0] == "R": - if v == 2: - tempDict["L"+k[1:]] = 2 - elif v == 0: - tempDict["L"+k[1:]] = 0 - - boxmode[self._widget.frameList.currentItem().text()] = deepcopy(tempDict) - self.updateTreeConfig(self._widget.treeModeSelector.currentIndex()) - self.box_ticked() - self._widget.statusBar.showMessage("Inverted frame") - - def frame_list(self): - ''' - Connects triggers from the frame list to their callback methods. - ''' - self._widget.frameList.itemSelectionChanged.connect(self.frame_select) - self._widget.lineFrameName.textEdited.connect(self.frame_meta_update) - self._widget.spinBoxPause.valueChanged.connect(self.frame_meta_update) - self._widget.spinBoxDuration.valueChanged.connect(self.frame_meta_update) - - def frame_meta_update(self): - ''' - Updates the respective values for the two spin boxes and the frame name, when they are changed - ''' - self._workingDuration = self._widget.spinBoxDuration.value() - self._workingPause = self._widget.spinBoxPause.value() - self._workingName = self._widget.lineFrameName.text() - - def frame_select(self): - ''' - Loads all information on a specific frame into the working values, if the frame selection changes - ''' - if not (self._widget.frameList.currentItem() == None): - self.copyOldTreeConfig() - selected_frame_name = self._widget.frameList.currentItem().text() - self._selected_frame = None - - - for v in self._recorder.get_animation_state(): - if v["name"] == selected_frame_name: - self._selected_frame = v - break - - #save current values to _currentValues if switching from current frame to different one - #when switching from current to different, save current values - - - if selected_frame_name == "#CURRENT_FRAME": - self._widget.treeModeSelector.setCurrentIndex(0) - self.updateTreeConfig(0) - self._widget.treeModeSelector.setEnabled(False) - self._workingValues = deepcopy(self._currentGoals) - self._workingName = deepcopy(self._currentName) - self._workingDuration = deepcopy(self._currentDuration) - self._workingPause = deepcopy(self._currentPause) - - self._current = True - else: - if self._selected_frame == None: - return - self._widget.treeModeSelector.setEnabled(True) - if self._current: - self._currentGoals = deepcopy(self._workingValues) - self._currentName = deepcopy(self._workingName) - self._currentDuration = deepcopy(self._workingDuration) - self._currentPause = deepcopy(self._workingPause) - - self._workingValues = self._selected_frame["goals"] - self._workingName = self._selected_frame["name"] - self._workingPause = self._selected_frame["pause"] - self._workingDuration = float(self._selected_frame["duration"]) - - self._widget.lineFrameName.setText(self._widget.frameList.currentItem().text()) - self._current = False - self.updateTreeConfig(self._previousTreeMode) - - if self._widget.treeModeSelector.currentIndex() == 0: - for k, v in self._treeItems.items(): - self._motorSwitched[k] = (v.checkState(0) == 2) - - - for k, v in self._motorSwitched.items(): - self._textFields[k].setEnabled(v) - self._sliders[k].setEnabled(v) - - self.box_ticked() - - def motor_switcher(self): - ''' - Loads the motors into the tree and adds the checkboxes - ''' - self._widget.motorTree.setHeaderLabel("Stiff Motors") - self._motorCheckBody.setText(0, "Body") - self._motorCheckBody.setFlags(self._motorCheckBody.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckBody.setExpanded(True) - self._motorCheckHead.setText(0, "Head") - self._motorCheckHead.setFlags(self._motorCheckHead.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckHead.setExpanded(True) - self._motorCheckArms.setText(0, "Arms") - self._motorCheckArms.setFlags(self._motorCheckArms.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckArms.setExpanded(True) - self._motorCheckLegs.setText(0, "Legs") - self._motorCheckLegs.setFlags(self._motorCheckLegs.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckLegs.setExpanded(True) - self._motorCheckLArm.setText(0, "Left Arm") - self._motorCheckLArm.setFlags(self._motorCheckLArm.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckLArm.setExpanded(True) - self._motorCheckRArm.setText(0, "Right Arm") - self._motorCheckRArm.setFlags(self._motorCheckRArm.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckRArm.setExpanded(True) - self._motorCheckLLeg.setText(0, "Left Leg") - self._motorCheckLLeg.setFlags(self._motorCheckLLeg.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckLLeg.setExpanded(True) - self._motorCheckRLeg.setText(0, "Right Leg") - self._motorCheckRLeg.setFlags(self._motorCheckRLeg.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable) - self._motorCheckRLeg.setExpanded(True) - - for k, v in self._currentGoals.items(): - parent = None - if 'LHip' in k or 'LKnee' in k or 'LAnkle' in k: - parent = self._motorCheckLLeg - elif 'RHip' in k or 'RKnee' in k or 'RAnkle' in k: - parent = self._motorCheckRLeg - elif 'LShoulder' in k or 'LElbow' in k: - parent = self._motorCheckLArm - elif 'RShoulder' in k or 'RElbow' in k: - parent = self._motorCheckRArm - elif 'Head' in k: - parent = self._motorCheckHead - child = QTreeWidgetItem(parent) - child.setText(0, k) - child.setFlags(child.flags() | Qt.ItemIsUserCheckable) - child.setCheckState(0, Qt.Checked) - self._treeItems[k] = child - - self._widget.motorTree.itemClicked.connect(self.box_ticked) - - def copyOldTreeConfig(self): - ''' - Saves the current configuration of the motor tree checkboxes into the corresponding dictionary - ''' - if not self._selected_frame == None: - tempDict={} - for k, v in self._treeItems.items(): - tempDict[k]= self._treeItems[k].checkState(0) - - if self._previousTreeMode == 1: - self._checkBoxesSave[self._selected_frame["name"]] = deepcopy(tempDict) - elif self._previousTreeMode == 0: - self._checkBoxesPower[self._selected_frame["name"]] = deepcopy(tempDict) - - def updateTreeConfig(self, index): - ''' - Loads the new configuration of the motor tree depending on the change - ''' - tempDict2={} - if not self._widget.frameList.currentItem() == None: - if not self._widget.frameList.currentItem().text() == "#CURRENT_FRAME": - if index == 1: - if self._selected_frame["name"] in self._checkBoxesSave.keys(): - tempDict2 = deepcopy(self._checkBoxesSave[self._selected_frame["name"]]) - else: - for k in self._workingValues: - tempDict2[k] = 2 - elif index == 0: - if self._selected_frame["name"] in self._checkBoxesPower.keys(): - tempDict2 = deepcopy(self._checkBoxesPower[self._selected_frame["name"]]) - else: - for k in self._workingValues: - tempDict2[k] = 2 - else: - for k in self._workingValues: - tempDict2[k] = 2 - self._previousTreeMode = index - for k, v in tempDict2.items(): - if v == 0: - self._treeItems[k].setCheckState(0, Qt.Unchecked) - elif v == 1: - self._treeItems[k].setCheckState(0, Qt.PartiallyChecked) - elif v == 2: - self._treeItems[k].setCheckState(0, Qt.Checked) - - def treeModeChanged(self, index): - self.copyOldTreeConfig() - self.updateTreeConfig(index) - - - def slider_update(self): - ''' - Updates all sliders, checks whether a value is too large, then replaces it - ''' - for k, v in self._sliders.items(): - self._workingValues[k] = math.radians(v.value()) - if self._workingValues[k] < -math.pi: - self._workingValues[k] = -math.pi - elif self._workingValues[k] > math.pi: - self._workingValues[k] = math.pi - self.set_sliders_and_text_fields(manual=True) - - def textfield_update(self): - ''' - Updates all textfields. - ''' - for k, v in self._textFields.items(): - try: - self._workingValues[k] = math.radians(float(v.text())) - except ValueError: - continue - self.set_sliders_and_text_fields(manual=True) - - def set_sliders_and_text_fields(self, manual): - ''' - Updates the text fields and sliders in self._sliders and self._textfields and also frame name and duration and pause - to the values in self._workingValues. - ''' - for k, v in self._workingValues.items(): - try: - if not self._treeItems[k].checkState(0) == 0: - self._textFields[k].setText(str(int(round(math.degrees(v))))) - self._sliders[k].setValue(int(round(math.degrees(v)))) - - except KeyError: - rospy.logwarn("Found a key-value pair for motor (%s), which doesn't seem to exist (yet). Ignoring it." % k) - self._widget.statusBar.showMessage("Found a key-value pair for motor (%s), which doesn't seem to exist (yet). Ignoring it.") - continue - except RuntimeError: - rospy.logwarn("Tried to access a PyQt element that no longer exists. This happens when you close the framework.") - self._widget.statusBar.showMessage("Tried to access a PyQt element that no longer exists. This happens when you close the framework.") - break - if manual: - self._widget.lineFrameName.setText(self._workingName) - self._widget.spinBoxDuration.setValue(self._workingDuration) - self._widget.spinBoxPause.setValue(self._workingPause) - - def box_ticked(self): - - ''' - Controls whether a checkbox has been clicked, and reacts. - ''' - for k, v in self._treeItems.items(): - self._motorSwitched[k] = (v.checkState(0) == 2) - - - for k, v in self._motorSwitched.items(): - self._textFields[k].setEnabled(v) - self._sliders[k].setEnabled(v) - - self.set_sliders_and_text_fields(manual=False) - - torque_msg = JointTorque() - torque_msg.joint_names = [] - torque_msg.on = [] - - if self._widget.treeModeSelector.currentIndex() == 0: - for k, v in sorted(self._treeItems.items()): - torque_msg.joint_names.append(k) - torque_msg.on.append(v.checkState(0) == 2) - - - pos_msg = JointCommand() - pos_msg.joint_names = [] - pos_msg.velocities = [1.0] * 20 - pos_msg.positions = [] - pos_msg.accelerations = [-1.0] * 20 - pos_msg.max_currents = [-1.0] * 20 - - for k,v in self._workingValues.items(): - pos_msg.joint_names.append(k) - pos_msg.positions.append(v) - - self._joint_pub.publish(pos_msg) - self.effort_pub.publish(torque_msg) - if not self._widget.frameList.currentItem() == None: - if not self._widget.frameList.currentItem().text() == "#CURRENT_FRAME": - self.treeModeChanged(self._widget.treeModeSelector.currentIndex()) - - def update_frames(self, keep=False): - - ''' - updates the list of frames present in the current animation - :return: - ''' - current_index = self._widget.frameList.currentIndex() - current_mode = self._widget.treeModeSelector.currentIndex() - current_state = self._recorder.get_animation_state() - while self._widget.frameList.takeItem(0): - continue - - for i in range(0, len(current_state)): - item = QListWidgetItem() - if "name" in current_state[i]: - item.setText(current_state[i]["name"]) - else: #older animations without names for the frames - item.setText(str(i)) - self._widget.frameList.addItem(item) - - current = QListWidgetItem() - current.setText("#CURRENT_FRAME") - self._widget.frameList.addItem(current) - if keep: - self._widget.frameList.setCurrentIndex(current_index) - self._widget.treeModeSelector.setCurrentIndex(current_mode) - else: - self._widget.frameList.setCurrentItem(current) - self._current = True - - def change_frame_order(self, new_order): - ''' Calls the recorder to update frame order and updates the gui''' - self._recorder.change_frame_order(new_order) - self.update_frames() - - def shutdown_plugin(self): - '''Clean up by sending the HCM a signal that we are no longer recording''' - self._joint_pub.publish(JointCommand()) - rospy.sleep(1) diff --git a/bitbots_rl_motion/launch/test.launch b/bitbots_rl_motion/launch/test.launch index 1dfef55cf..89da53dbd 100644 --- a/bitbots_rl_motion/launch/test.launch +++ b/bitbots_rl_motion/launch/test.launch @@ -6,10 +6,25 @@ - + + + + + + + + + + + + + + + + diff --git a/bitbots_rl_motion/package.xml b/bitbots_rl_motion/package.xml index 52d1032a4..da4769d63 100644 --- a/bitbots_rl_motion/package.xml +++ b/bitbots_rl_motion/package.xml @@ -15,10 +15,12 @@ ament_cmake - rclpy - std_msgs bitbots_docs + bitbots_robot_description + bitbots_utils diagnostic_aggregator + rclpy + std_msgs diff --git a/bitbots_splines/include/bitbots_splines/polynom.h b/bitbots_splines/include/bitbots_splines/polynom.h index 640b36416..093ae874a 100644 --- a/bitbots_splines/include/bitbots_splines/polynom.h +++ b/bitbots_splines/include/bitbots_splines/polynom.h @@ -15,7 +15,7 @@ namespace bitbots_splines { /** * Polynom * - * Simple one dimentional + * Simple one dimensional * polynom class for spline * generation */ @@ -23,7 +23,7 @@ class Polynom { public: /** - * Default and inital degree initialization + * Default and initial degree initialization */ Polynom(); explicit Polynom(unsigned int degree); @@ -73,7 +73,7 @@ class Polynom { private: /** - * Polynom coeficients + * Polynom coefficients */ std::vector coefs_; };