From 75b8c764bcf489c880527d08808e14c6888691a1 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 11 Aug 2023 22:13:25 +0200 Subject: [PATCH 01/20] Cleanup misc --- Jenkinsfile | 13 - bitbots_bringup/.rdmanifest | 12 - .../launch/README.md | 2 +- .../launch/audio.launch | 0 bitbots_bringup/launch/base.launch | 14 + bitbots_bringup/launch/game_start.launch | 4 - bitbots_bringup/launch/highlevel.launch | 4 +- .../launch/monitoring.launch | 0 bitbots_bringup/launch/motion.launch | 21 + bitbots_bringup/launch/motion_base.launch | 16 + .../launch/motion_standalone.launch | 8 +- .../launch/move_group.py | 0 .../launch/rosbag_record.launch.py | 4 +- bitbots_bringup/launch/rviz_multirobot.launch | 10 - bitbots_bringup/launch/teamplayer.launch | 2 +- .../launch/tools.launch | 4 +- .../launch/vision_standalone.launch | 2 +- bitbots_bringup/launch/visualization.launch | 12 +- bitbots_bringup/package.xml | 12 +- bitbots_bringup/repair.sh | 15 - bitbots_ceiling_cam/.rdmanifest | 11 - bitbots_ceiling_cam/launch/ceiling_cam.launch | 2 - bitbots_convenience_frames/.rdmanifest | 20 - bitbots_convenience_frames/CMakeLists.txt | 2 - .../convenience_frames.h | 8 +- .../launch/convenience_frames.launch | 4 - bitbots_convenience_frames/package.xml | 1 - .../src/convenience_frames.cpp | 58 - bitbots_teleop/.rdmanifest | 15 - bitbots_teleop/bitbots_teleop/joy_node.py | 5 +- bitbots_teleop/scripts/teleop_keyboard.py | 45 +- bitbots_utils/CMakeLists.txt | 21 + bitbots_utils/bitbots_utils/transforms.py | 21 +- bitbots_utils/bitbots_utils/utils.py | 1 + .../config/game_settings_options.yaml | 2 +- bitbots_utils/config/multirobot.rviz | 1228 ----------------- bitbots_utils/launch/base.launch | 47 - bitbots_utils/package.xml | 14 +- bitbots_utils/scripts/boot-highlevel.sh | 12 - bitbots_utils/scripts/boot-motion.sh | 12 - bitbots_utils/scripts/boot-roscore.sh | 12 - bitbots_utils/scripts/boot-teamplayer.sh | 12 - bitbots_utils/scripts/boot-vision.sh | 12 - bitbots_utils/scripts/dummy_imu.py | 2 - bitbots_utils/scripts/extract_profiler.py | 20 - .../scripts/goals_to_joint_states.py | 23 - bitbots_utils/scripts/keep_stable_in_sim.py | 87 -- .../scripts/motor_goals_viz_helper.py | 2 +- .../scripts/publish_ball_relative.py | 51 - bitbots_utils/scripts/rosbag_to_pandas.py | 184 --- bitbots_utils/scripts/set_motor_position.py | 36 - bitbots_utils/scripts/tf_delay_plot.cpp | 61 + 52 files changed, 203 insertions(+), 1983 deletions(-) delete mode 100644 Jenkinsfile delete mode 100644 bitbots_bringup/.rdmanifest rename {bitbots_utils => bitbots_bringup}/launch/README.md (88%) rename {bitbots_utils => bitbots_bringup}/launch/audio.launch (100%) create mode 100644 bitbots_bringup/launch/base.launch delete mode 100644 bitbots_bringup/launch/game_start.launch rename {bitbots_utils => bitbots_bringup}/launch/monitoring.launch (100%) create mode 100644 bitbots_bringup/launch/motion_base.launch rename {bitbots_utils => bitbots_bringup}/launch/move_group.py (100%) rename {bitbots_utils => bitbots_bringup}/launch/rosbag_record.launch.py (93%) delete mode 100644 bitbots_bringup/launch/rviz_multirobot.launch rename {bitbots_utils => bitbots_bringup}/launch/tools.launch (74%) delete mode 100755 bitbots_bringup/repair.sh delete mode 100644 bitbots_ceiling_cam/.rdmanifest delete mode 100644 bitbots_convenience_frames/.rdmanifest delete mode 100644 bitbots_teleop/.rdmanifest delete mode 100644 bitbots_utils/config/multirobot.rviz delete mode 100644 bitbots_utils/launch/base.launch delete mode 100755 bitbots_utils/scripts/boot-highlevel.sh delete mode 100755 bitbots_utils/scripts/boot-motion.sh delete mode 100755 bitbots_utils/scripts/boot-roscore.sh delete mode 100755 bitbots_utils/scripts/boot-teamplayer.sh delete mode 100755 bitbots_utils/scripts/boot-vision.sh delete mode 100755 bitbots_utils/scripts/extract_profiler.py delete mode 100755 bitbots_utils/scripts/goals_to_joint_states.py delete mode 100755 bitbots_utils/scripts/keep_stable_in_sim.py delete mode 100755 bitbots_utils/scripts/publish_ball_relative.py delete mode 100755 bitbots_utils/scripts/rosbag_to_pandas.py delete mode 100755 bitbots_utils/scripts/set_motor_position.py create mode 100644 bitbots_utils/scripts/tf_delay_plot.cpp diff --git a/Jenkinsfile b/Jenkinsfile deleted file mode 100644 index 29ee0efa..00000000 --- a/Jenkinsfile +++ /dev/null @@ -1,13 +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_bringup"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_ceiling_cam")).withoutDocumentation()) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_convenience_frames"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_live_tool_rqt"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_teleop"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_time_constraint"))) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("system_monitor"))) -pipeline.execute() diff --git a/bitbots_bringup/.rdmanifest b/bitbots_bringup/.rdmanifest deleted file mode 100644 index ce11239f..00000000 --- a/bitbots_bringup/.rdmanifest +++ /dev/null @@ -1,12 +0,0 @@ ---- -# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci -check-presence-script: '#!/bin/bash - - test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_bringup' -depends: -- bitbots_docs -exec-path: bitbots_misc-master/bitbots_bringup -install-script: '#!/bin/bash - - cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_bringup' -uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz diff --git a/bitbots_utils/launch/README.md b/bitbots_bringup/launch/README.md similarity index 88% rename from bitbots_utils/launch/README.md rename to bitbots_bringup/launch/README.md index 4eb68704..87ead6c0 100644 --- a/bitbots_utils/launch/README.md +++ b/bitbots_bringup/launch/README.md @@ -3,7 +3,7 @@ Launch these commands in parallel: ```bash -ros2 launch bitbots_utils audio.launch +ros2 launch bitbots_bringup audio.launch ros2 bag record /audio/audio /audio/audio_info ``` diff --git a/bitbots_utils/launch/audio.launch b/bitbots_bringup/launch/audio.launch similarity index 100% rename from bitbots_utils/launch/audio.launch rename to bitbots_bringup/launch/audio.launch diff --git a/bitbots_bringup/launch/base.launch b/bitbots_bringup/launch/base.launch new file mode 100644 index 00000000..af69895c --- /dev/null +++ b/bitbots_bringup/launch/base.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/bitbots_bringup/launch/game_start.launch b/bitbots_bringup/launch/game_start.launch deleted file mode 100644 index 01f2e66b..00000000 --- a/bitbots_bringup/launch/game_start.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/bitbots_bringup/launch/highlevel.launch b/bitbots_bringup/launch/highlevel.launch index db750d70..c89d2f1c 100644 --- a/bitbots_bringup/launch/highlevel.launch +++ b/bitbots_bringup/launch/highlevel.launch @@ -50,7 +50,7 @@ - + @@ -81,7 +81,7 @@ - + diff --git a/bitbots_utils/launch/monitoring.launch b/bitbots_bringup/launch/monitoring.launch similarity index 100% rename from bitbots_utils/launch/monitoring.launch rename to bitbots_bringup/launch/monitoring.launch diff --git a/bitbots_bringup/launch/motion.launch b/bitbots_bringup/launch/motion.launch index e6d38a63..0882c3fb 100644 --- a/bitbots_bringup/launch/motion.launch +++ b/bitbots_bringup/launch/motion.launch @@ -5,6 +5,27 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/bitbots_bringup/launch/motion_base.launch b/bitbots_bringup/launch/motion_base.launch new file mode 100644 index 00000000..452663d1 --- /dev/null +++ b/bitbots_bringup/launch/motion_base.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/bitbots_bringup/launch/motion_standalone.launch b/bitbots_bringup/launch/motion_standalone.launch index 1c152072..4263585f 100644 --- a/bitbots_bringup/launch/motion_standalone.launch +++ b/bitbots_bringup/launch/motion_standalone.launch @@ -3,8 +3,12 @@ - - + + + + + + diff --git a/bitbots_utils/launch/move_group.py b/bitbots_bringup/launch/move_group.py similarity index 100% rename from bitbots_utils/launch/move_group.py rename to bitbots_bringup/launch/move_group.py diff --git a/bitbots_utils/launch/rosbag_record.launch.py b/bitbots_bringup/launch/rosbag_record.launch.py similarity index 93% rename from bitbots_utils/launch/rosbag_record.launch.py rename to bitbots_bringup/launch/rosbag_record.launch.py index 1d2363bf..cfa6ad5b 100644 --- a/bitbots_utils/launch/rosbag_record.launch.py +++ b/bitbots_bringup/launch/rosbag_record.launch.py @@ -6,9 +6,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction -from launch.conditions import IfCondition -from launch.substitutions import EnvironmentVariable, EqualsSubstitution, LaunchConfiguration, NotEqualsSubstitution, PathJoinSubstitution -from launch_ros.actions import Node +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution TOPICS_TO_RECORD: List[str] = [ diff --git a/bitbots_bringup/launch/rviz_multirobot.launch b/bitbots_bringup/launch/rviz_multirobot.launch deleted file mode 100644 index f8499827..00000000 --- a/bitbots_bringup/launch/rviz_multirobot.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/bitbots_bringup/launch/teamplayer.launch b/bitbots_bringup/launch/teamplayer.launch index 7697647a..3cf96162 100644 --- a/bitbots_bringup/launch/teamplayer.launch +++ b/bitbots_bringup/launch/teamplayer.launch @@ -15,7 +15,7 @@ - + diff --git a/bitbots_utils/launch/tools.launch b/bitbots_bringup/launch/tools.launch similarity index 74% rename from bitbots_utils/launch/tools.launch rename to bitbots_bringup/launch/tools.launch index ca61966c..464deed6 100644 --- a/bitbots_utils/launch/tools.launch +++ b/bitbots_bringup/launch/tools.launch @@ -5,11 +5,11 @@ - + - + diff --git a/bitbots_bringup/launch/vision_standalone.launch b/bitbots_bringup/launch/vision_standalone.launch index dc0fc8d8..c91dd30d 100644 --- a/bitbots_bringup/launch/vision_standalone.launch +++ b/bitbots_bringup/launch/vision_standalone.launch @@ -6,7 +6,7 @@ - + diff --git a/bitbots_bringup/launch/visualization.launch b/bitbots_bringup/launch/visualization.launch index 73091c00..530aca60 100644 --- a/bitbots_bringup/launch/visualization.launch +++ b/bitbots_bringup/launch/visualization.launch @@ -3,15 +3,11 @@ - - - - - + @@ -25,12 +21,15 @@ - + + + + @@ -41,5 +40,4 @@ - diff --git a/bitbots_bringup/package.xml b/bitbots_bringup/package.xml index 9fa1d747..29cc6a82 100644 --- a/bitbots_bringup/package.xml +++ b/bitbots_bringup/package.xml @@ -14,22 +14,32 @@ bitbots_docs + wolfgang_webots_sim + audio_common bitbots_basler_camera bitbots_body_behavior + bitbots_convenience_frames bitbots_dynamic_kick bitbots_dynup bitbots_hcm bitbots_head_mover bitbots_localization + bitbots_odometry bitbots_quintic_walk bitbots_ros_control bitbots_utils bitbots_vision + demo_nodes_cpp + humanoid_base_footprint robot_state_publisher - wolfgang_webots_sim soccer_ipm + system_monitor + udp_bridge + wolfgang_description + wolfgang_moveit_config xacro + unknown diff --git a/bitbots_bringup/repair.sh b/bitbots_bringup/repair.sh deleted file mode 100755 index 91b551ff..00000000 --- a/bitbots_bringup/repair.sh +++ /dev/null @@ -1,15 +0,0 @@ -touch devel/lib/python2.7/dist-packages/bitbots_cm730/lowlevel/controller/__init__.py - -touch devel/lib/python2.7/dist-packages/bitbots_cm730/lowlevel/__init__.py - -touch devel/lib/python2.7/dist-packages/bitbots_cm730/__init__.py - -touch devel/lib/python2.7/dist-packages/bitbots_walking/__init__.py - -touch devel/lib/python2.7/dist-packages/bitbots_bringup/pose/__init__.py - -touch devel/lib/python2.7/dist-packages/bitbots_bringup/utilCython/__init__.py - -cp src/bitbots_misc/humanoid_league_speaker/src/humanoid_league_speaker/cfg/speaker_paramsConfig.py* ./devel/lib/python2.7/dist-packages/humanoid_league_speaker/cfg - -cp src/bitbots_lowlevel/bitbots_hcm/src/bitbots_hcm/cfg/motion_paramsConfig.py* ./devel/lib/python2.7/dist-packages/bitbots_hcm/cfg diff --git a/bitbots_ceiling_cam/.rdmanifest b/bitbots_ceiling_cam/.rdmanifest deleted file mode 100644 index 059279fc..00000000 --- a/bitbots_ceiling_cam/.rdmanifest +++ /dev/null @@ -1,11 +0,0 @@ ---- -# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci -check-presence-script: '#!/bin/bash - - test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_ceiling_cam' -depends: null -exec-path: bitbots_misc-master/bitbots_ceiling_cam -install-script: '#!/bin/bash - - cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_ceiling_cam' -uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz diff --git a/bitbots_ceiling_cam/launch/ceiling_cam.launch b/bitbots_ceiling_cam/launch/ceiling_cam.launch index 3c0ae281..1a3ef673 100644 --- a/bitbots_ceiling_cam/launch/ceiling_cam.launch +++ b/bitbots_ceiling_cam/launch/ceiling_cam.launch @@ -20,13 +20,11 @@ - diff --git a/bitbots_convenience_frames/.rdmanifest b/bitbots_convenience_frames/.rdmanifest deleted file mode 100644 index 812eeb55..00000000 --- a/bitbots_convenience_frames/.rdmanifest +++ /dev/null @@ -1,20 +0,0 @@ ---- -# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci -check-presence-script: '#!/bin/bash - - test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_convenience_frames' -depends: -- bitbots_docs -- geometry_msgs -- humanoid_league_msgs -- roscpp -- rospy -- std_msgs -- tf2 -- tf2_geometry_msgs -- tf2_ros -exec-path: bitbots_misc-master/bitbots_convenience_frames -install-script: '#!/bin/bash - - cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_convenience_frames' -uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz diff --git a/bitbots_convenience_frames/CMakeLists.txt b/bitbots_convenience_frames/CMakeLists.txt index 4b694b82..a9431ae5 100644 --- a/bitbots_convenience_frames/CMakeLists.txt +++ b/bitbots_convenience_frames/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(rclcpp REQUIRED) find_package(ament_cmake REQUIRED) find_package(tf2_ros REQUIRED) find_package(biped_interfaces REQUIRED) -find_package(humanoid_league_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(bitbots_docs REQUIRED) @@ -28,7 +27,6 @@ ament_target_dependencies(convenience_frames ament_cmake bitbots_docs biped_interfaces - humanoid_league_msgs rclcpp std_msgs tf2 diff --git a/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h b/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h index 0fe4042b..36fafc68 100644 --- a/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h +++ b/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h @@ -5,8 +5,6 @@ #include #include #include -#include -#include #include #include @@ -22,14 +20,10 @@ class ConvenienceFramesBroadcaster : public rclcpp::Node { std::unique_ptr tfBuffer_{std::make_unique(this->get_clock())}; tf2_ros::TransformListener tfListener_{*tfBuffer_, this}; - std::string base_link_frame_, r_sole_frame_, l_sole_frame_, r_toe_frame_, l_toe_frame_, approach_frame_, - ball_frame_, right_post_frame_, left_post_frame_, general_post_frame_; + std::string base_link_frame_, r_sole_frame_, l_sole_frame_, r_toe_frame_, l_toe_frame_, approach_frame_; bool is_left_support{false}; bool got_support_foot_{false}; void publishTransform(std::string header_frame_id, std::string child_frame_id, double x, double y, double z); void supportFootCallback(const biped_interfaces::msg::Phase::SharedPtr msg); - void ballsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg); - void goalCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg); - void goalPostsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg); }; diff --git a/bitbots_convenience_frames/launch/convenience_frames.launch b/bitbots_convenience_frames/launch/convenience_frames.launch index 34864162..8eb1a3c5 100644 --- a/bitbots_convenience_frames/launch/convenience_frames.launch +++ b/bitbots_convenience_frames/launch/convenience_frames.launch @@ -10,9 +10,5 @@ - - - - \ No newline at end of file diff --git a/bitbots_convenience_frames/package.xml b/bitbots_convenience_frames/package.xml index a3286c54..c077fa42 100644 --- a/bitbots_convenience_frames/package.xml +++ b/bitbots_convenience_frames/package.xml @@ -20,7 +20,6 @@ tf2_geometry_msgs tf2_ros geometry_msgs - humanoid_league_msgs diff --git a/bitbots_convenience_frames/src/convenience_frames.cpp b/bitbots_convenience_frames/src/convenience_frames.cpp index 1f739d52..5fd1332a 100644 --- a/bitbots_convenience_frames/src/convenience_frames.cpp +++ b/bitbots_convenience_frames/src/convenience_frames.cpp @@ -14,14 +14,6 @@ ConvenienceFramesBroadcaster::ConvenienceFramesBroadcaster() : Node("convenience this->get_parameter("l_toe_frame", l_toe_frame_); this->declare_parameter("approach_frame", "approach_frame"); this->get_parameter("approach_frame", approach_frame_); - this->declare_parameter("ball_frame", "ball"); - this->get_parameter("ball_frame", ball_frame_); - this->declare_parameter("right_post_frame", "right_post"); - this->get_parameter("right_post_frame", right_post_frame_); - this->declare_parameter("left_post_frame", "left_post"); - this->get_parameter("left_post_frame", left_post_frame_); - this->declare_parameter("general_post_frame", "post_"); - this->get_parameter("general_post_frame", general_post_frame_); got_support_foot_ = false; rclcpp::Subscription::SharedPtr walking_support_foot_subscriber = @@ -34,21 +26,6 @@ ConvenienceFramesBroadcaster::ConvenienceFramesBroadcaster() : Node("convenience 1, std::bind(&ConvenienceFramesBroadcaster::supportFootCallback, this, _1)); - rclcpp::Subscription::SharedPtr ball_relative_subscriber = - this->create_subscription("balls_relative", - 1, - std::bind(&ConvenienceFramesBroadcaster::ballsCallback, - this, _1)); - rclcpp::Subscription::SharedPtr goal_relative_subscriber = - this->create_subscription("goal_relative", - 1, - std::bind(&ConvenienceFramesBroadcaster::goalCallback, - this, _1)); - rclcpp::Subscription::SharedPtr goal_posts_relative_subscriber = - this->create_subscription("goal_posts_relative", - 1, - std::bind(&ConvenienceFramesBroadcaster::goalPostsCallback, - this, _1)); } void ConvenienceFramesBroadcaster::loop() { rclcpp::Time last_published_time; @@ -192,41 +169,6 @@ void ConvenienceFramesBroadcaster::supportFootCallback(const biped_interfaces::m is_left_support = (msg->phase == biped_interfaces::msg::Phase::LEFT_STANCE); } -void ConvenienceFramesBroadcaster::ballsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg) { - for (humanoid_league_msgs::msg::PoseWithCertainty ball: msg->poses) { - publishTransform(msg->header.frame_id, ball_frame_, - ball.pose.pose.position.x, - ball.pose.pose.position.y, - ball.pose.pose.position.z); - } -} - -void ConvenienceFramesBroadcaster::goalCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg) { - if (msg->poses.size() > 0) { - publishTransform(msg->header.frame_id, - left_post_frame_, - msg->poses[0].pose.pose.position.x, - msg->poses[0].pose.pose.position.y, - msg->poses[0].pose.pose.position.z); - } - if (msg->poses.size() > 2) { - publishTransform(msg->header.frame_id, - right_post_frame_, - msg->poses[1].pose.pose.position.x, - msg->poses[1].pose.pose.position.y, - msg->poses[1].pose.pose.position.z); - } -} - -void ConvenienceFramesBroadcaster::goalPostsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg) { - for (size_t i = 0; i < msg->poses.size(); i++) { - publishTransform(msg->header.frame_id, - general_post_frame_ + std::to_string(i), - msg->poses[i].pose.pose.position.x, - msg->poses[i].pose.pose.position.y, - msg->poses[i].pose.pose.position.z); - } -} void ConvenienceFramesBroadcaster::publishTransform(std::string header_frame_id, std::string child_frame_id, double x, double y, double z) { diff --git a/bitbots_teleop/.rdmanifest b/bitbots_teleop/.rdmanifest deleted file mode 100644 index b097aae6..00000000 --- a/bitbots_teleop/.rdmanifest +++ /dev/null @@ -1,15 +0,0 @@ ---- -# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci -check-presence-script: '#!/bin/bash - - test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_teleop' -depends: -- bitbots_docs -- humanoid_league_msgs -- rospy -- std_msgs -exec-path: bitbots_misc-master/bitbots_teleop -install-script: '#!/bin/bash - - cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_teleop' -uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz diff --git a/bitbots_teleop/bitbots_teleop/joy_node.py b/bitbots_teleop/bitbots_teleop/joy_node.py index 41b913e4..ad726a37 100755 --- a/bitbots_teleop/bitbots_teleop/joy_node.py +++ b/bitbots_teleop/bitbots_teleop/joy_node.py @@ -145,13 +145,13 @@ def set_head_mode(self, mode): msg.head_mode = mode self.head_mode_pub.publish(msg) - def denormalize_joy(self, gain, axis, msg, deadzone=0.0): + def denormalize_joy(self, gain, axis, msg: Joy, deadzone=0.0): if abs(msg.axes[axis]) > deadzone: return gain * msg.axes[axis] else: return 0 - def joy_cb(self, msg): + def joy_cb(self, msg: Joy): # forward and sideward walking with left joystick self.walk_msg.linear.x = float(self.denormalize_joy( self.config['walking']['gain_x'], @@ -220,7 +220,6 @@ def main(): rclpy.init(args=None) node = JoyNode() rclpy.spin(node) - node.destroy_node() rclpy.shutdown() diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py index f9b061e6..e3be40c0 100755 --- a/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_teleop/scripts/teleop_keyboard.py @@ -12,6 +12,7 @@ from geometry_msgs.msg import Twist from sensor_msgs.msg import JointState from bitbots_msgs.msg import JointCommand +from bitbots_utils.transforms import quat_from_yaw import sys, select, termios, tty from rclpy.action import ActionClient @@ -113,12 +114,12 @@ Walk around: Move head: q w e u i o a s d j k l - m , . + m , . q/e: turn left/right k: zero head position a/d: left/rigth i/,: up/down w/s: forward/back j/l: left/right - u/o/m/.: combinations + u/o/m/.: combinations Controls increase / decrease with multiple presses. SHIFT increases with factor 10 @@ -231,11 +232,11 @@ def __init__(self): else: self.head_pub = self.create_publisher(JointCommand, "DynamixelController/command", 1) self.head_msg = JointCommand() - self.head_msg.max_currents = [float(-1)] * 2 - self.head_msg.velocities = [float(5)] * 2 - self.head_msg.accelerations = [float(40)] * 2 + self.head_msg.max_currents = [-1.0] * 2 + self.head_msg.velocities = [5.0] * 2 + self.head_msg.accelerations = [40.0] * 2 self.head_msg.joint_names = ['HeadPan', 'HeadTilt'] - self.head_msg.positions = [float(0)] * 2 + self.head_msg.positions = [0.0] * 2 self.head_pan_step = 0.05 self.head_tilt_step = 0.05 @@ -262,15 +263,9 @@ def generate_kick_goal(self, x, y, direction): kick_goal = Kick.Goal() kick_goal.header.stamp = self.get_clock().now().to_msg() kick_goal.header.frame_id = self.frame_prefix + "base_footprint" - kick_goal.ball_position.x = float(x) - kick_goal.ball_position.y = float(y) - kick_goal.ball_position.z = float(0) - x, y, z, w = quaternion_from_euler(0, 0, direction) - kick_goal.kick_direction.x = float(x) - kick_goal.kick_direction.y = float(y) - kick_goal.kick_direction.z = float(z) - kick_goal.kick_direction.w = float(w) - kick_goal.kick_speed = float(1) + kick_goal.ball_position = Vector3(x=float(x), y=float(y), z=0.0) + kick_goal.kick_direction = quat_from_yaw(direction) + kick_goal.kick_speed = 1.0 return kick_goal def joint_state_cb(self, msg): @@ -376,12 +371,8 @@ def loop(self): break twist = Twist() - twist.linear.x = float(self.x) - twist.linear.y = float(self.y) - twist.linear.z = float(0) - twist.angular.x = float(self.a_x) - twist.angular.y = float(0) - twist.angular.z = float(self.th) + twist.linear = Vector3(x=float(self.x), y=float(self.y), z=0.0) + twist.angular = Vector3(x=float(self.a_x), y=0.0, z=float(self.th)) self.pub.publish(twist) sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") @@ -397,12 +388,12 @@ def loop(self): finally: print("\n") twist = Twist() - twist.linear.x = float(0) - twist.linear.y = float(0) - twist.linear.z = float(0) - twist.angular.x = float(-1) - twist.angular.y = float(0) - twist.angular.z = float(0) + twist.linear.x = 0.0 + twist.linear.y = 0.0 + twist.linear.z = 0.0 + twist.angular.x = -1.0 + twist.angular.y = 0.0 + twist.angular.z = 0.0 self.pub.publish(twist) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) diff --git a/bitbots_utils/CMakeLists.txt b/bitbots_utils/CMakeLists.txt index 95b964b8..c9534c28 100644 --- a/bitbots_utils/CMakeLists.txt +++ b/bitbots_utils/CMakeLists.txt @@ -4,11 +4,32 @@ project(bitbots_utils) find_package(bitbots_docs REQUIRED) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) + + +set(INCLUDE_DIRS include) +include_directories(${INCLUDE_DIRS}) + +set(CMAKE_CXX_STANDARD 17) +add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra) enable_bitbots_docs() +add_executable(tf_delay_plot scripts/tf_delay_plot.cpp) + +ament_target_dependencies(tf_delay_plot + ament_cmake + rclcpp + tf2_msgs + std_msgs) + ament_python_install_package(${PROJECT_NAME}) +install(TARGETS tf_delay_plot + DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) diff --git a/bitbots_utils/bitbots_utils/transforms.py b/bitbots_utils/bitbots_utils/transforms.py index 77484b46..e4877ad9 100644 --- a/bitbots_utils/bitbots_utils/transforms.py +++ b/bitbots_utils/bitbots_utils/transforms.py @@ -1,25 +1,28 @@ import math import numpy as np -from transforms3d.quaternions import quat2mat, mat2quat +from geometry_msgs.msg import Quaternion +from ros2_numpy import msgify from transforms3d.euler import quat2euler, euler2quat +from transforms3d.quaternions import quat2mat, mat2quat from transforms3d.quaternions import rotate_vector, qinverse, quat2mat, mat2quat -def wxyz2xyzw(quat_wxyz): - return np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]]) + +def wxyz2xyzw(quat_wxyz: np.ndarray) -> np.ndarray: + return quat_wxyz[[1,2,3,0]] -def xyzw2wxyz(quat_xyzw): - return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) +def xyzw2wxyz(quat_xyzw: np.ndarray) -> np.ndarray: + return quat_xyzw[[3,0,1,2]] -def quat2sixd(quat_wxyz): +def quat2sixd(quat_wxyz: np.ndarray) -> np.ndarray: # see https://openaccess.thecvf.com/content_CVPR_2019/supplemental/Zhou_On_the_Continuity_CVPR_2019_supplemental.pdf # first get matrix m = quat2mat(quat_wxyz) # 6D represenation is first 2 coloumns of matrix - return [m[0][0], m[1][0], m[2][0], m[0][1], m[1][1], m[2][1]] + return m[[[0][0], m[1][0], m[2][0], m[0][1], m[1][1], m[2][1]]] def sixd2quat(sixd): @@ -112,3 +115,7 @@ def compute_imu_orientation_from_world(robot_quat_in_world): rp = rotate_vector((yrp_world_frame[1], yrp_world_frame[2], 0), qinverse(yaw_quat)) # save in correct order return [rp[0], rp[1], 0], yaw_quat + + +def quat_from_yaw(yaw: float) -> Quaternion: + return msgify(Quaternion, wxyz2xyzw(euler2quat(0, 0, yaw, axes='szxy'))) diff --git a/bitbots_utils/bitbots_utils/utils.py b/bitbots_utils/bitbots_utils/utils.py index ac3b0467..71562bc3 100644 --- a/bitbots_utils/bitbots_utils/utils.py +++ b/bitbots_utils/bitbots_utils/utils.py @@ -150,6 +150,7 @@ def set_parameters_of_other_node(own_node: Node, response = future.result() return [res.success for res in response.results] + def parse_parameter_dict(*, namespace, parameter_dict): parameters = [] for param_name, param_value in parameter_dict.items(): diff --git a/bitbots_utils/config/game_settings_options.yaml b/bitbots_utils/config/game_settings_options.yaml index c31d10a0..f91bb156 100644 --- a/bitbots_utils/config/game_settings_options.yaml +++ b/bitbots_utils/config/game_settings_options.yaml @@ -16,7 +16,7 @@ team_color: type: int explanation: "0 is blue, 1 is red" role: - options: ['goalie', 'offense', 'defense', 'penalty'] + options: ['goalie', 'offense', 'defense'] type: str explanation: "" position_number: diff --git a/bitbots_utils/config/multirobot.rviz b/bitbots_utils/config/multirobot.rviz deleted file mode 100644 index 3468e9f8..00000000 --- a/bitbots_utils/config/multirobot.rviz +++ /dev/null @@ -1,1228 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 698 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.699999988079071 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /amy/field/map - Unreliable: false - Use Timestamp: false - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - head: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_frame_2: - Alpha: 1 - Show Axes: false - Show Trail: false - l_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - l_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - l_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - llb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - llf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - neck: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - r_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - r_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - rlb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rlf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: /amy/robot_description - TF Prefix: /amy - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - head: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_frame_2: - Alpha: 1 - Show Axes: false - Show Trail: false - l_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - l_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - l_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - llb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - llf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - neck: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - r_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - r_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - rlb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rlf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: /rory/robot_description - TF Prefix: /rory - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - head: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_frame_2: - Alpha: 1 - Show Axes: false - Show Trail: false - l_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - l_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - l_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - llb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - llf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - neck: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - r_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - r_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - rlb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rlf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: /jack/robot_description - TF Prefix: /jack - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - head: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_frame_2: - Alpha: 1 - Show Axes: false - Show Trail: false - l_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - l_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - l_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - l_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - l_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - l_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - llb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - llf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - neck: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_ankle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_cleat_l_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_l_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_back: - Alpha: 1 - Show Axes: false - Show Trail: false - r_cleat_r_front: - Alpha: 1 - Show Axes: false - Show Trail: false - r_foot: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_hip_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_lower_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_shoulder: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_sole: - Alpha: 1 - Show Axes: false - Show Trail: false - r_toe: - Alpha: 1 - Show Axes: false - Show Trail: false - r_upper_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_upper_leg: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - r_wrist: - Alpha: 1 - Show Axes: false - Show Trail: false - rlb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rlf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrb: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rrf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - torso: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: /donna/robot_description - TF Prefix: /donna - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /amy/move_base/current_goal - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /rory/move_base/current_goal - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /jack/move_base/current_goal - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /donna/move_base/current_goal - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /amy/move_base/NavfnROS/plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /rory/move_base/NavfnROS/plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /jack/move_base/NavfnROS/plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /donna/move_base/NavfnROS/plan - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 12.589564323425293 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.06874994933605194 - Y: -0.4793209135532379 - Z: -0.45180970430374146 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5403993129730225 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.5404052734375 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 995 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000345fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000345000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000345fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000345000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064d0000003efc0100000002fb0000000800540069006d006501000000000000064d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003dc0000034500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1613 - X: 67 - Y: 27 diff --git a/bitbots_utils/launch/base.launch b/bitbots_utils/launch/base.launch deleted file mode 100644 index 19800956..00000000 --- a/bitbots_utils/launch/base.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bitbots_utils/package.xml b/bitbots_utils/package.xml index 050b1a9e..8ba9911d 100644 --- a/bitbots_utils/package.xml +++ b/bitbots_utils/package.xml @@ -15,21 +15,11 @@ ament_cmake bitbots_docs - - bitbots_convenience_frames - bitbots_extrinsic_calibration - bitbots_odometry - demo_nodes_cpp - humanoid_base_footprint - robot_state_publisher - wolfgang_description - wolfgang_moveit_config - xacro - audio_common python3-yaml python3-transforms3d python3-numpy - topic_tools + geometry_msgs + ros2_numpy diff --git a/bitbots_utils/scripts/boot-highlevel.sh b/bitbots_utils/scripts/boot-highlevel.sh deleted file mode 100755 index db25e301..00000000 --- a/bitbots_utils/scripts/boot-highlevel.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# -# This script gets executed on boot if the systemd service 'start_behavior.service' is enabled -# - -if hash roslaunch 2>/dev/null; then - roslaunch bitbots_bringup highlevel.launch -else - echo "Please source the workspace" - exit 1 -fi diff --git a/bitbots_utils/scripts/boot-motion.sh b/bitbots_utils/scripts/boot-motion.sh deleted file mode 100755 index 3c6c4446..00000000 --- a/bitbots_utils/scripts/boot-motion.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# -# This script gets executed on boot if the systemd service 'start_roscore.service' is enabled -# - -if hash roslaunch 2>/dev/null; then - roslaunch bitbots_bringup motion_standalone.launch -else - echo "Please source the workspace" - exit 1 -fi diff --git a/bitbots_utils/scripts/boot-roscore.sh b/bitbots_utils/scripts/boot-roscore.sh deleted file mode 100755 index 97b92372..00000000 --- a/bitbots_utils/scripts/boot-roscore.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# -# This script gets executed on boot if the systemd service 'start_roscore.service' is enabled -# - -if hash roscore 2>/dev/null; then - roscore -else - echo "Please source the workspace" - exit 1 -fi diff --git a/bitbots_utils/scripts/boot-teamplayer.sh b/bitbots_utils/scripts/boot-teamplayer.sh deleted file mode 100755 index 596139c5..00000000 --- a/bitbots_utils/scripts/boot-teamplayer.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# -# This script gets executed on boot if the systemd service 'start_behavior.service' is enabled -# - -if hash roslaunch 2>/dev/null; then - roslaunch bitbots_bringup teamplayer.launch -else - echo "Please source the workspace" - exit 1 -fi diff --git a/bitbots_utils/scripts/boot-vision.sh b/bitbots_utils/scripts/boot-vision.sh deleted file mode 100755 index 29564541..00000000 --- a/bitbots_utils/scripts/boot-vision.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# -# This script gets executed on boot if the systemd service 'start_vision.service' is enabled -# - -if hash roslaunch 2>/dev/null; then - roslaunch bitbots_vision vision_startup.launch -else - echo "Please source the workspace" - exit 1 -fi diff --git a/bitbots_utils/scripts/dummy_imu.py b/bitbots_utils/scripts/dummy_imu.py index 9198ea90..e86075a5 100755 --- a/bitbots_utils/scripts/dummy_imu.py +++ b/bitbots_utils/scripts/dummy_imu.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -import threading - import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor diff --git a/bitbots_utils/scripts/extract_profiler.py b/bitbots_utils/scripts/extract_profiler.py deleted file mode 100755 index a1abf73b..00000000 --- a/bitbots_utils/scripts/extract_profiler.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from swri_profiler_msgs.msg import ProfileDataArray -from bitbots_msgs.msg import ProfileDataStamped - -""" -This script extracts data from the SWRI profiler to later use it in combination with the rosbag_to_pandas script. -""" -rclpy.init(args=None) -pub = self.create_publisher(ProfileDataStamped, "/profiler/single_data", 1) - - -def cb(msg): - pub.publish(ProfileDataStamped(msg.header, msg.rostime_stamp, msg.data[0])) - - -rospy.Subscriber("/profiler/data", ProfileDataArray, cb) -rclpy.spin(self) diff --git a/bitbots_utils/scripts/goals_to_joint_states.py b/bitbots_utils/scripts/goals_to_joint_states.py deleted file mode 100755 index 0b96fc92..00000000 --- a/bitbots_utils/scripts/goals_to_joint_states.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf8 -*- -import rospy -from bitbots_msgs.msg import JointCommand -from sensor_msgs.msg import JointState - - -def goal_cb(msg): - state_msg = JointState() - state_msg.name = msg.joint_names - state_msg.position = msg.positions - state_msg.header = msg.header - state_msg.header.stamp = rospy.Time.now() - - state_publisher.publish(state_msg) - - -if __name__ == "__main__": - rospy.init_node('goals_to_joint_states') - state_publisher = rospy.Publisher("/joint_states", JointState, queue_size=1) - sub = rospy.Subscriber("/DynamixelController/command", JointCommand, callback=goal_cb, queue_size=1, - tcp_nodelay=True) - rospy.spin() diff --git a/bitbots_utils/scripts/keep_stable_in_sim.py b/bitbots_utils/scripts/keep_stable_in_sim.py deleted file mode 100755 index 1d26d8a9..00000000 --- a/bitbots_utils/scripts/keep_stable_in_sim.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from bitbots_msgs.msg import FootPressure -from gazebo_msgs.msg import ModelStates, ModelState -from gazebo_msgs.srv import SetModelState, SetModelStateRequest - -import tf - - -position = None -roll = None -pitch = None -yaw = None - - -def state_update(state_msg): - global position - global roll - global pitch - global yaw - global ball_pose - global ball_twist - - index = 0 - for name in state_msg.name: - if name == "/": - - position = state_msg.pose[index].position - orientation = state_msg.pose[index].orientation - quaternion = ( - orientation.x, - orientation.y, - orientation.z, - orientation.w) - euler = tf.transformations.euler_from_quaternion(quaternion) - roll = euler[0] - pitch = euler[1] - yaw = euler[2] - elif name == "teensize_ball": - - ball_pose = state_msg.pose[index] - ball_twist = state_msg.twist[index] - - index += 1 - - -if __name__ == "__main__": - rclpy.init(args=None) - rospy.wait_for_service("/gazebo/set_model_state") - set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) - - - goal_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, state_update, tcp_nodelay=True) - - request = SetModelStateRequest() - request.model_state.model_name = "/" - ball_request = SetModelStateRequest() - ball_request.model_state.model_name = "teensize_ball" - # wait because we want to be called - rospy.sleep(1.0) - rate = self.create_rate(10) - while rclpy.ok(): - try: - # check if we have values already, otherwise we will do math with none - if(yaw): - request.model_state.pose.position = position - request.model_state.pose.position.z = 0.43 # the robot is not and will not be ready for take off. - quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw) - request.model_state.pose.orientation.x = quaternion[0] - request.model_state.pose.orientation.y = quaternion[1] - request.model_state.pose.orientation.z = quaternion[2] - request.model_state.pose.orientation.w = quaternion[3] - set_state(request) - if ball_pose: - ball_request.model_state.pose = ball_pose - ball_request.model_state.pose.position.z = 0.095 - ball_request.model_state.twist = ball_twist - set_state(ball_request) - - rate.sleep() - except rospy.exceptions.ROSTimeMovedBackwardsException: - rospy.logwarn( - "We moved backwards in time. I hope you just resetted the simulation. If not there is something wrong") - except rospy.exceptions.ROSInterruptException: - exit() diff --git a/bitbots_utils/scripts/motor_goals_viz_helper.py b/bitbots_utils/scripts/motor_goals_viz_helper.py index 261e90d1..762f9f38 100755 --- a/bitbots_utils/scripts/motor_goals_viz_helper.py +++ b/bitbots_utils/scripts/motor_goals_viz_helper.py @@ -109,7 +109,7 @@ def animation_cb(self, msg: Animation): self.joint_command_msg.positions[self.joint_names.index(name)] = msg.position.points[0].positions[i] self.joint_command_msg.velocities[self.joint_names.index(name)] = -1 - def update_joint_states(self, msg): + def update_joint_states(self, msg: JointState): for i in range(len(msg.joint_names)): name = msg.joint_names[i] if msg.velocities[i] == -1: diff --git a/bitbots_utils/scripts/publish_ball_relative.py b/bitbots_utils/scripts/publish_ball_relative.py deleted file mode 100755 index bc5d7c52..00000000 --- a/bitbots_utils/scripts/publish_ball_relative.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf8 -*- - -import rclpy -from rclpy.node import Node -import sys -import random -from humanoid_league_msgs.msg import PoseWithCertainty, PoseWithCertaintyArray - -delta = 1 -max_x = 10 -min_x = 0.3 -max_y = 10 -min_y = -10 -max_z = 0 -min_z = 0 - -if __name__ == "__main__": - rclpy.init(args=None) - balls_relative_publisher = self.create_publisher(PoseWithCertaintyArray, "balls_relative", 10, tcp_nodelay=True) - - x = random.uniform(min_x, max_x) / 4 - y = random.uniform(min_y, max_y) / 4 - z = random.uniform(min_z, max_z) / 4 - - while rclpy.ok(): - ball_msg = PoseWithCertainty() - if len(sys.argv) is in [3, 4]: - ball_msg.pose.pose.position.x = float(sys.argv[1]) - ball_msg.pose.pose.position.y = float(sys.argv[2]) - if len(sys.argv) == 4: - ball_msg.pose.pose.position.z= float(sys.argv[3]) - else: - ball_msg.ball_relative.x = x - ball_msg.ball_relative.y = y - ball_msg.ball_relative.z = z - - # New position for next step - x = max(min_x, min(x + delta * random.uniform(-1, 1), max_x)) - y = max(min_y, min(y + delta * random.uniform(-1, 1), max_y)) - z = max(min_z, min(z + delta * random.uniform(-1, 1), max_z)) - - ball_msg.confidence = 1.0 - - balls_msg = PoseWithCertaintyArray() - balls_msg.header.stamp = self.get_clock().now() - balls_msg.poses = [ball_msg] - balls_relative_publisher.publish(balls_msg) - - rospy.sleep(0.5) -0.5) diff --git a/bitbots_utils/scripts/rosbag_to_pandas.py b/bitbots_utils/scripts/rosbag_to_pandas.py deleted file mode 100755 index ed7747ba..00000000 --- a/bitbots_utils/scripts/rosbag_to_pandas.py +++ /dev/null @@ -1,184 +0,0 @@ -#!/usr/bin/env python3 - -import rosbag -import argparse -import rosmsg -import pandas as pd -import os.path -import pickle - -FILE_PATH = "/tmp/rosbag_to_pandas_input" - -""" -This script reads in a rosbag and ouputs a pandas dataframe representration of the data. This is usefull for later -processing, e.g. in scikit-learn or for creating plots. -""" - - -class COLORS: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKGREEN = '\033[92m' - WARNING = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' - - -parser = argparse.ArgumentParser() -parser.add_argument("bag") -parser.add_argument("--output-folder", required=False) - -args = parser.parse_args() - -bag = rosbag.Bag(args.bag) -topics = [] -types = [] -for key, value in bag.get_type_and_topic_info().topics.items(): - topics.append(key) - types.append(value[0]) - -# see if we have a previous set of inputs -use_saved = False -if os.path.isfile(FILE_PATH): - use_saved_input = input("Do you want to extract the same data as last time? [Y|n]") - use_saved = use_saved_input in ["y", "Y", ""] - if use_saved: - saved_input = pickle.load(open(FILE_PATH, "rb")) -else: - print("No previous input found. You will need to enter everything.") - -if use_saved: - topic_selections_str = saved_input["topic_selections_str"] -else: - col_width = max(len(word) for row in [topics, types] for word in row) + 4 # - for i in range(len(topics)): - print(f"{COLORS.BOLD}{COLORS.OKBLUE}[{i}]:{COLORS.ENDC} {topics[i].ljust(col_width)}{types[i]}") - topic_selections_str = input("Select data source. Use a format like 0 2 3.\n") - -topic_selections_str_list = topic_selections_str.split() -topic_selections = [] - -for topic_selection_str in topic_selections_str_list: - try: - topic_selections.append(int(topic_selection_str)) - except ValueError as ex: - print(ex) - exit(-1) - -selected_topics_list = [topics[i] for i in topic_selections] -# used later to know how far you have been -message_count = sum(bag.get_message_count(topics[i]) for i in topic_selections) - -if use_saved: - data_selections = saved_input["data_selections"] -else: - data_selections = [] - for topic_selection in topic_selections: - print( - f"{COLORS.BOLD}{COLORS.OKGREEN}Available Fields from {COLORS.OKBLUE}{topics[topic_selection]}{COLORS.ENDC}") - print(rosmsg.get_msg_text(types[topic_selection])) - single_topic_data_selection = input("Specify which fields you want seperated with spaces. " - "You can also use a semantic like \"orientation.x\" to access internal fields." - "Header stamp and seq are included automatically.\n") - data_selections.append(single_topic_data_selection.split()) - - -def recursive_getattr(obj, field_list): - if len(field_list) == 1: - return getattr(obj, field_list[0]) - else: - return recursive_getattr(getattr(obj, field_list[0]), field_list[1:]) - - -frames = [] -for data_selection in data_selections: - frames.append(pd.DataFrame(columns=["header"].extend(data_selection))) - -msg_generator = bag.read_messages(topics=[topics[i] for i in topic_selections]) - -if use_saved: - seperate_input = saved_input["seperate_input"] -else: - seperate_input = input("Do you want to extract lists and tuples into separate columns? [Y|n]") -seperate = seperate_input in ["y", "Y", ""] -if seperate: - print("Will separate\n") -else: - print("Will not separate\n") - -if use_saved: - remove_remove_time_offset_input = saved_input["remove_remove_time_offset_input"] -else: - remove_remove_time_offset_input = input("Do you want the time to start at 0 (remove time offset)? [Y|n]") -remove_time_offset = remove_remove_time_offset_input in ["y", "Y", ""] -if remove_time_offset: - print("Will remove offset\n") -else: - print("Will not remove offset\n") - -print(f"{COLORS.BOLD}{COLORS.WARNING}Extracting data{COLORS.ENDC}") -header_warning_printed = False -current_msg_index = 0 -first_stamp = [None] * len(selected_topics_list) -for msg in msg_generator: - # give some status feedback since this can take a while - if current_msg_index % 1000 == 0: - print(f"{current_msg_index} / {message_count}") - current_msg_index += 1 - - try: - i = selected_topics_list.index(msg.topic) - except ValueError: - continue - - fields = {} - try: - time_stamp = msg.message.header.stamp.to_sec() - except AttributeError: - if not header_warning_printed: - header_warning_printed = True - print( - f"{COLORS.WARNING}No header found in one of the messages. Will use receiving time as stamp.{COLORS.ENDC}") - time_stamp = msg.timestamp.to_sec() - if not first_stamp[i]: - # remember first timestamp to remove offset - first_stamp[i] = time_stamp - # remove offset if wanted - if remove_time_offset: - fields["stamp"] = time_stamp - first_stamp[i] - else: - fields["stamp"] = time_stamp - - for data_selection in data_selections[i]: - f = recursive_getattr(msg.message, data_selection.split(".")) - # extract lists in own rows - if seperate and (isinstance(f, list) or isinstance(f, tuple)): - j = 0 - for entry in f: - fields[f"{data_selection}_{j}"] = entry - j += 1 - else: - fields[data_selection] = f - frames[i] = frames[i].append(fields, ignore_index=True) - -if args.output_folder: - output_folder = args.output_folder - if not os.path.exists(output_folder): - os.makedirs(output_folder) -else: - output_folder = "." - -for i, frame in enumerate(frames): - print(f"{COLORS.OKBLUE}{selected_topics_list[i]}{COLORS.ENDC}") - print(frame.info()) - frame.to_pickle(output_folder + "/" + selected_topics_list[i][1:].replace("/", "-") + ".pickle") - -# save user input for next time -input_to_save = {} -input_to_save["topic_selections_str"] = topic_selections_str -input_to_save["data_selections"] = data_selections -input_to_save["seperate_input"] = seperate_input -input_to_save["remove_remove_time_offset_input"] = remove_remove_time_offset_input -pickle.dump(input_to_save, open(FILE_PATH, "wb")) diff --git a/bitbots_utils/scripts/set_motor_position.py b/bitbots_utils/scripts/set_motor_position.py deleted file mode 100755 index 447808ac..00000000 --- a/bitbots_utils/scripts/set_motor_position.py +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf8 -*- -from math import pi - -import rclpy -from rclpy.node import Node -import time -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint - -if __name__ == "__main__": - rclpy.init(args=None) - ns = rospy.get_param("ns") - joint_goal_publisher = self.create_publisher(JointTrajectory, ns +'/controller/command', 10, tcp_nodelay=True) - - i = -1.5 - while rclpy.ok(): - msg = JointTrajectoryPoint() - msg.positions = [i, -0.4] - msg.time_from_start.nsecs = 10000000 - traj_msg = JointTrajectory() - # make an array with String objects (ros message type) - traj_msg.joint_names = ['HeadPan', 'HeadTilt'] - traj_msg.points = [] - traj_msg.points.append(msg) - traj_msg.header.stamp = self.get_clock().now() - - rospy.logwarn(traj_msg) - - joint_goal_publisher.publish(traj_msg) - rospy.sleep(0.2) - if i < 1.5: - i += 0.1 - else: - i = -1.5 - - rclpy.spin(self) \ No newline at end of file diff --git a/bitbots_utils/scripts/tf_delay_plot.cpp b/bitbots_utils/scripts/tf_delay_plot.cpp new file mode 100644 index 00000000..3df79761 --- /dev/null +++ b/bitbots_utils/scripts/tf_delay_plot.cpp @@ -0,0 +1,61 @@ +// This script logs the delay of certain tf frames and publishes them to be visualized in plotjuggler. + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::placeholders; + +class TfDelayPlot : public rclcpp::Node { + + public: + TfDelayPlot() : Node("tf_delay_plot", rclcpp::NodeOptions()) { + RCLCPP_INFO(this->get_logger(), "Starting tf_delay_plot"); + // Create correct qos profile + tf_sub_ = this->create_subscription( + "/tf", + 5, + std::bind(&TfDelayPlot::tf_callback, this, _1)); + } + + void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Got transform message"); + for (auto &transform: msg->transforms) { + // Calculate delay + double delay = ((transform.header.stamp.sec * 1.0e9 + transform.header.stamp.nanosec) - this->now().nanoseconds()) / 1.0e9; + + if (std::abs(delay) > 0.0001) { + // Print + RCLCPP_INFO(this->get_logger(), "Frame: %s, Delay: %f", transform.child_frame_id.c_str(), delay); + + // Print current time + RCLCPP_INFO(this->get_logger(), "Current time: %f", this->now().seconds()); + + // Print stamp + RCLCPP_INFO(this->get_logger(), "Stamp: %f", transform.header.stamp.sec + transform.header.stamp.nanosec / 1.0e9); + } + } + } + + private: + rclcpp::Subscription::SharedPtr tf_sub_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + + +/// How do I set simtime to true while running a cpp node directly? + +// Answer: + +// ros2 run demo_nodes_cpp talker --ros-args --use-sim-time \ No newline at end of file From 8d40d3767c1cf7e46a37fd5d350d7639d63097a5 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 11 Aug 2023 22:19:39 +0200 Subject: [PATCH 02/20] Shorten code --- bitbots_teleop/scripts/teleop_keyboard.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py index e3be40c0..585d391c 100755 --- a/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_teleop/scripts/teleop_keyboard.py @@ -388,12 +388,7 @@ def loop(self): finally: print("\n") twist = Twist() - twist.linear.x = 0.0 - twist.linear.y = 0.0 - twist.linear.z = 0.0 twist.angular.x = -1.0 - twist.angular.y = 0.0 - twist.angular.z = 0.0 self.pub.publish(twist) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) From 89f06eccea42e828f4e7d45b8ba2c3a8951badbb Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 11 Aug 2023 23:54:57 +0200 Subject: [PATCH 03/20] Fix launch files --- bitbots_bringup/launch/motion.launch | 2 +- .../launch/motion_standalone.launch | 4 +- bitbots_bringup/launch/teamplayer.launch | 4 +- .../launch/vision_standalone.launch | 4 +- bitbots_bringup/launch/visualization.launch | 4 +- bitbots_bringup/package.xml | 5 +- bitbots_robot_description/CMakeLists.txt | 19 ++ .../docs/_static/logo.png | Bin 0 -> 11686 bytes bitbots_robot_description/docs/conf.py | 193 ++++++++++++++++++ bitbots_robot_description/docs/index.rst | 21 ++ .../launch/load_robot_description.launch | 4 +- .../launch/move_group.py | 3 +- bitbots_robot_description/package.xml | 30 +++ .../launch/parameter_blackboard.launch | 2 - bitbots_utils/package.xml | 1 + bitbots_utils/scripts/tf_delay_plot.cpp | 7 - 16 files changed, 276 insertions(+), 27 deletions(-) create mode 100644 bitbots_robot_description/CMakeLists.txt create mode 100644 bitbots_robot_description/docs/_static/logo.png create mode 100644 bitbots_robot_description/docs/conf.py create mode 100644 bitbots_robot_description/docs/index.rst rename bitbots_bringup/launch/motion_base.launch => bitbots_robot_description/launch/load_robot_description.launch (57%) rename {bitbots_bringup => bitbots_robot_description}/launch/move_group.py (98%) create mode 100644 bitbots_robot_description/package.xml rename bitbots_bringup/launch/base.launch => bitbots_utils/launch/parameter_blackboard.launch (67%) diff --git a/bitbots_bringup/launch/motion.launch b/bitbots_bringup/launch/motion.launch index 0882c3fb..e52c3a55 100644 --- a/bitbots_bringup/launch/motion.launch +++ b/bitbots_bringup/launch/motion.launch @@ -6,7 +6,7 @@ - + diff --git a/bitbots_bringup/launch/motion_standalone.launch b/bitbots_bringup/launch/motion_standalone.launch index 4263585f..01cd78d0 100644 --- a/bitbots_bringup/launch/motion_standalone.launch +++ b/bitbots_bringup/launch/motion_standalone.launch @@ -4,11 +4,11 @@ - + - + diff --git a/bitbots_bringup/launch/teamplayer.launch b/bitbots_bringup/launch/teamplayer.launch index 3cf96162..899784c3 100644 --- a/bitbots_bringup/launch/teamplayer.launch +++ b/bitbots_bringup/launch/teamplayer.launch @@ -14,8 +14,8 @@ - - + + diff --git a/bitbots_bringup/launch/vision_standalone.launch b/bitbots_bringup/launch/vision_standalone.launch index c91dd30d..38839102 100644 --- a/bitbots_bringup/launch/vision_standalone.launch +++ b/bitbots_bringup/launch/vision_standalone.launch @@ -5,8 +5,8 @@ - - + + diff --git a/bitbots_bringup/launch/visualization.launch b/bitbots_bringup/launch/visualization.launch index 530aca60..fc9f3bea 100644 --- a/bitbots_bringup/launch/visualization.launch +++ b/bitbots_bringup/launch/visualization.launch @@ -6,8 +6,8 @@ - - + + diff --git a/bitbots_bringup/package.xml b/bitbots_bringup/package.xml index 29cc6a82..aed24330 100644 --- a/bitbots_bringup/package.xml +++ b/bitbots_bringup/package.xml @@ -26,19 +26,16 @@ bitbots_localization bitbots_odometry bitbots_quintic_walk + bitbots_robot_description bitbots_ros_control bitbots_utils bitbots_vision - demo_nodes_cpp humanoid_base_footprint - robot_state_publisher soccer_ipm system_monitor udp_bridge wolfgang_description wolfgang_moveit_config - xacro - diff --git a/bitbots_robot_description/CMakeLists.txt b/bitbots_robot_description/CMakeLists.txt new file mode 100644 index 00000000..c192f1a3 --- /dev/null +++ b/bitbots_robot_description/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(bitbots_robot_description) + +find_package(bitbots_docs REQUIRED) +find_package(ament_cmake REQUIRED) + + +set(INCLUDE_DIRS include) +include_directories(${INCLUDE_DIRS}) + +set(CMAKE_CXX_STANDARD 17) +add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra) + +enable_bitbots_docs() + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/bitbots_robot_description/docs/_static/logo.png b/bitbots_robot_description/docs/_static/logo.png new file mode 100644 index 0000000000000000000000000000000000000000..f8afdd5d066232e2dba94b758c6c44fe72b42820 GIT binary patch literal 11686 zcmXw92RN1Q|9;Jm5r>RpWG7@(BpJsDp=`1phm^hd-h0QfqL3Xzj!i~JRv}wfGBUII zKi}VV{om_2=klKSea`bfgp&VAb|Sc&F*NFgB@tx<-Z;YfUD5ico@ud|H^*oUSla|u+Kdso~i3-C87>d;4E{yfHSMIeSYOAHl#@;HngrMIR za(p~|u$?+a2JyvKOCTfT;B1gttk6vv#`gO?`gt~(xpedquJ|CL=|PKn?m9DD=mk^Y z`pvEr56IzTL$yfX%#R0!t^&gc*Om=4+dse|CnvY0%~O{v^gtfA1liK!FWrLd>t?nK z23GeY3%FpxffosI;X?I+nzx%0qG}G}@BNLfK7d%1$HcNQ z7^x>Pa})}-B+n*Pxp@XIxVsR(qwlQ3YSMhQs<^3m)2aPq+MK?l*ksrL8ST zs;06I?Xet3C`*od*ZRSb87}nw`*$TZH8lp$TxlfCiI+zxR+Pcfu z^bsrm_VzY{iAj-*s?#FwyAnt6-k#IAQqWj*6bG+pi&z_s5(nii z>D=|vk&!^nqv67*Z;)Rt_vO;jEt+E?cPupMWMMzwa`g|~)2lq>d(UJ%c~?R-Q&rul zI5l|P%aQTMxTraTkawWJ;U;L+RU)5@n2YZj@g_E&gqHvC54JT}$clI6@mgnrFp{cU zD<>z1!P3Aa^^9*I*c>6Kc8jq$X#KS2kuN`fNXM7HzCP1G*efyWD#-{1tqI0nzo~8a z20ofwfm@Mq1&Rk*Hw(DUe_v5;`x;4!( zoWhx;;6;k~C{99CCnLi`M~Qz`k?C<~hKZTk8V?U|VN!HEH(E!6C~dpYw@~uh5zL?Q zoktde5|NIaq$f~)R7dky`~wjg4GAiReDz|NhlPigDienFKeTmp>gJdCF3N_i zYIz5Q`35MF`6Jl+`UfS+`09#+^j{p;1qYe4TwXW4!P)S*Q@&qO`e8LSqH+_4k6VJl zV8%n?DzN+wR}2|B`9hv5=fI0$t{5;|6)i0-m)(Z;jp!dMn-OHUk@IbB`iN*U^Uo+m zPXN6Pjsw9dYtZ32rLJy;5f$eOMNIdgag)26kP_K#3D?uRs?@MTu-%4(UCFJE(m&mFL@8hlNXs10|83`h z$+%XA+3?o2CV#jS>N!0FvW~s8PevAkOZ?#`Cv`AW)rjgcZ0 zUclAg_I+Z)KN0?Gt{T;wE!PUyqC+8~6e)z&5JyKJcRu4yL{rPC-4qnYo&G`KeIj^b z2A7JLi<>dljF`Z_is2`*q#I#~VckCNx|m&W-)Uv|P>Ok>q$A|L2=(OoQeKp3>90A; zWEP#TodtU3_U_Cz-L7gq`=bs`KFaxKNE!77&qX0>@F5!`S#MrleZ4<-j(W^Yy&c~0 z!AD+_+K2l^_coOzh;|dCh>ddDqoJ2uYsDxAmm3ArRH6pb`MQ3T=}{!~CMj_^t_{*D9|{p zG$PtwQPwv-8=F;ypgi)4H?8>K1UMfR^wM=_|BeL%C zIDZ)O*5++&CA&nN;y}&rT9yM_rYaj#!g7Dwy_c_F%dsWFTTK4?C36^y8H&^jBhy#g zD$2@Ur*jZJ^4JMwfhiS_gUIFP=7wNq&g04gz1x}+@|0wP+S96qLXUUC8UwB`NHfUt z$k(neVSndh?x7Smu@8k1F|4lV+k$z156W)z2OhQKDP3MJERX%>g4&n&7Ekt;BfIA@ zI}u3MlS}{SDoN_a9Zrn%r{6p}t!nctD=QU1KXNOsg0(4-n5?WUF-jy04EMrcn#pU0 zLR-&zZjW60VmFjBrgvG~`;ZHSGRTaBUcq;mDI?*r?gYuYxzsyM94^Y9%-MUkm@qzaCpE0M4I1Cb)>rmD9DO1SljWj9 z>gi2}nz%L1iNTQ=IuzdcCcpT9YeuiEtZXx=oOeZ>%%qAoRa5eK*2S^WC{mscq@t4_ zRsIB3lO#x(t8s6lP3OhgB<;l*WafXli4Y#*W~WAU4JAKwkEzN)*AF0L)g_< ziSceRx%i{khLmLLJvkAlQs}51mI94PGLeHE$&-c)o%ptTD`;GOdfq}%l7c?P`aXG- zqkge?L%MybD99t;k?Y#?}o=ij^tCTQb2PbL}yRR|(~+6uNF@g1g>2y|8>z8g0M zGB}izK86-uJAJo=4p-$h{p~Jxm%1FA<`Tgk&LhVZE%ha_dtRNcFF7~D6MBUnJRr;( z(tZAH@FNIf@!{d&BvN@*nako})>H}?-J7bdhUgLDLG$w_q@<*(ygJj{?q6;Z8QFm) zIQG<~t*;NZo3#M>`L+M~(S9_Ecc8CyaogiYEUhBu=q%29k)T3q~dx=?zC(E_PrU(LnN#Kc4%PqeUHzzSb@^F4OTD4ArA zDcc9x47U~5ervpKZ53QCR>xt!5)_mo%iv5@#pS0pGcd4{JRlV`9MKbmc@5B%AJMjv z3Nl5#J(gn*KGF-&`P@z(0P|fFG4L*mk;=W%Y_gcOfG@S5)H%#7MLK*}I*P1;VZ)Wa zv{X2uS1xdi5mn##TsTt{+thUY2kURPe?(Uhwfwnh%iTxzjyiYFuV)sqL7JGj_a7>9 za)?Y%RVFN(ed!Rl33_@UouRASWOz5oaW^ON&W~yRTU(i|?6yOV_z6*=?Pu*bh!-w) zJc6G2vOSU@*^T%!*u|kt#+OOqrA) zd=n&fWQ=&phU2o(+8Jizi365Qjp@9t;Lujgf(J7kM#W#_z+4a|-{BLMsI6l*UX&?D zcq2&qLXJiE3y&5rq3`K{jKkkSA4&3h=R4XuIgw>NgrRWIxYXrYEE6q$e&YxFBx)VT zURFg|Inw{lp3|aMUVr~D3pg2`$AauoU%x5RQQq5Oq zo1eXpc%b$TSBv4?$C2F@00x$ia6BK6X3j}TQnHMT*@&0UBe&vK9b%pA)jIR{E_Mk` zET9uw>M9n!w_dzFn3)_U&r4{86+OV(Sk1`lKIDlGo>#7U#FW6EsoJ^Z7`X10n0?QJ zxZkx74|<<@uU!-l4WlO`$|(`ao6FoJPs0!zu9BL$6u7-Oa&jUhv%W6o4o+QzVB@E(!d!-?s%=-0rR^x9Flz++1#?(^#A?@AHU+ zK^=>OxHI+WNA*FBu4FY*r5AfV3(JdefpLEI+yp~Ny5 zBs_2^tM*pQM(AyEXp+Obb4l>epFg`v3BLHuAecEV$B9|I9Q)u(gzNZwWbi@jep=_5 z?Sm8FcGHXQ!OgE6zeh&CL}nyV#0tG1w~>FRai1o}CZh-D|6)lB+1FEDUHu5>0*_Vx zT`DG3v&V^%8b3&nDol^6+ooZrB=df**PC&ovt{P4i-ET6Y3v-Ct{}!C>`qE zmcP`^3desVUMJuCB1me~|G{pr%+DzXICC;fs7b?Wf_?x{Fjgu0itw< z4Y&cv9__Sk8Fl)^-1oEi&c~!L%qQvuZ`1P2#CEq+3o762;u2qWm&k#ecoD31$F!H) z;M^6Phh>Z7Y|Y#-Y7|c6SE+3+K@SA3=iQjL9&CZs@UHvVKjeMNttfp}3KCk9tHh-g z)MO+5I-aWZ;eBN~7AJ9~}xgpP%q+F=^f-^ z*q8!OyE&}eeHr(JX|zH3UJpZ97BhW6mRdc#ueIC;_u*;`xgXc2Zx)&;ec=;PPNwSD znMwc^vDxZ~+Ce-BYC9otTv5`E6(Zc4!f9T<-M%wdOHY3|V5&IhGMq1U7b%iSip65d zN=vaZa7}`@BtJitX&{vpsT7pc(La|QBi);f*St)~!jO;Oy?b~2RN^iI&_?fPZf-{& z&fL_T?Fc!}6dcqbKraUe4CgTQ8zzr3O%N02JG z!%PA}DrMF%Q+4Ls+NM@KK4AACOD!D@a;19uD$;=RP>I8N0l>UGLT5m)`x+Mf&%as< zH%jE>1|)b0XUYp_`aXoiE}q)iZ8tD5Fr*o+ia!#@y*%6~|6st?X%1s}u!zNAYC&07H27(nHCopH!P9!&4c#VHrIeKnA_pM3vB_n zP`q+DG8(Bv4fEcc`xY6H<+}nyaL}Oyp0FipUr7{F4Y@!1?DLmzDDF8donU9)XVkVq zfGaprGnyvr=0!zA`WgyD*3AUhQGtG zJ(Z7|NkR6tFp>X(70Ws$H_@K>=i^q|);Fe#1D`ag;}qQnYI1QM=YX8V5PkHhq%=A` zod$r#ofesR3j2npJG!xk;y2=#pPQQg{W!CI6mIT&moiE<^WlB$+{Wd*UJnLZ^ zV*zM!%V1Y*A-1PXZ}fBE+n^1{d&IiCk}ElnIxa_`k z8!KP*?CL`Ot}cd?(dRh$`lL4~N!Ij zxbI31?~81owj;VmM&O_`fwPjxNw=2b;^N;YS_bJ(|F(WEf!H{SK(WfgTN#MMr2ZbC z7jCTTeii=J8*jcM$BY~k?T~oXF+JUm!N}m^B<3CYB->cpSNj6?z2f7gg#F9vN8 zyDVSMEi5xH-GFn=<0CD)WCvM`q*De3- zSwaT_#n_t!`r0}=I;s{c*VC552u6g!+@)cu$`Tq`!edH6v#glRi@J2Motnr zUp8C;VE9;PHH1+`q24`SpKf1VT-0afgaRu!J#LbcUMQxo#&-`69lM*kx{BV8QzWOd zz=Z^RD1mJEjzP@6{FR*@wWWa~_+8V_jpvi8>k1;8KWv`$uJQC9 zToB|9-^dzDPl%~2DV6Ao+}PgkK0R3XE8w!nb7lwYSIlARBaTYYlqL{{o95!RdBRfe zJ|ipCEb-)}+02YV^i1ifY&>bgVsj1aUg;bKJSlrA>|CN$wSo0#K-qp9In?KMUu_S< zqup|Ea_+h*COWh2{((SPvHsJix22`T#n1BWapx)3;{F+AzYaJ54ITxSEAI5GTZUy9 zhT=6M-2{$79pg|viOPzH>jN!$2ST73k#-w z(*E;Oc+<_UjA~LP?yx!g8?5hfJYy)XeyXbXWVl_N?Bx;S2Hha0$-o1T@uc)ugimd%wae4|yw^51OWW?}^GLL3?h~mFVge|B*(%4( zN|+Ai=(xM28JR(*xL^ih@1f2oyTBw}qff@alfb*XCnzYlo%q=|8J50bF)=_Kn=!Trn_n`QMWS zZ^u?=*kD>}=khvXx$D+wpmFd;7nB>qgdj76iK_c*O{YK_dDGj|lP>z#z1l1`zo9`) zLR9pyz#Jha(nB0dl1`|gF)>gxD?D@QyDo%M(&_u}A=b z6@fqmwuRcg=w{1Ne^*pQ7+t{9CJhq)q-nrqou{PKx_pIdqR|4iJpVH;_Ed+?Q+7J^{;1jCuhb=CcqMO5FbpkZ8)y%h$FK~FZs0vw* zAIAHrjei>Muepc`3JRJG?pQpBw6#>cWZ@Yc%FSHSH5|(BvT!B!2}4ZL9UYyaml&%J zth%$PsZ}iBxZ5RR@%X7fw7|dX&xGt#huR8K{j=bjT?oUE_-3&ie0+ReQ2Ma0A{?J# zKQ@uWMqLpg99*H})gU+<&WEG(OKBT$Ks5vLnW}{9#kUH$IyaBz*Y-SHdz`XK+V@mB z5AHTvpQ>_dF(>pgK%$x|hT`#t9f+q{MP8XgG?3%Xc9uXmDwYDNtDJo3@@C*D3h<+a zHylJjODNGMPSph3Te)eQzlW*m0=Bial{!`Pl|lLq8I8*9)3>Fsg5ua6WT_M$LF5h% zGn-zydG`swh^l=koi-#YfB>y5*u-jSq`CqSLrEOw2-sGqs2X5WMlIgdn-dkACr4EQ zEwRg=Q%g%5peb^w(e?tTm@Jxy+G>;8lvl(nE$VB{5e}iqd;??oM$3J-RJf*~d8G{oY^e6BsSJat z5`QuiPwwrB;qb>q`BUIxIY_JHoqL>E+i~;-D$zS6TE)vumgTz z&(oI;J;HG&!(F$RIz~q`J~f5dX}k8nI4qXl{`>cD-W@sx1N{3dvxt(ty}iO4aeNQ+V(nbH`674g>jFvqV5jPRu6?I z!I7%tR9?9b1cCo12z@VKn?fkkyvxffbIIgDa|EkpLs=$cua0Pr2#Oc$zCEo4y?9_h z8~OANzxh5sX5A~(nNbWa4>-T=d@~)W5Ydz2Oo$U=X(3OS)5c(U8IW-xi{^#kAi zdGhx4EQ@kjXkzj|KU!oZnIoeZ#hU>V$$E#GP&-30^{sCOsaVL94#ipc>DT(+;qkGw z^Y4-Tr7U}_O}OVTarHHqTjfN6VRQ*Pg3SAF&$IbU##y5*_YoXP3W3^q4%E&Y9?`ac zGcq`^z}KQDk~r!PU^bFSc~ovVensVq6)yqmmo zX&=VF?oTsKEYyYVd0upa9GGA`hdsg@?-C|rec*o<3CG77mOL2D1i4h8}Fep5{q z$y7~V_u|h{Kfdkh<(%xG1d3)~Yj}e+`zeq#pO3gU%Q-?{Zn#RkFMS0{W>&-WnEF=ra~>G0=z5n| z7r@qWy~|DG8;27%=xO21!I*IK3ntc;KIE|fKmsg&IX9|z8QLYsR$kjW(Gfz@yN5zI2@lWE_)zt|!&S@oQ+`Cv&VAavl(9yXA;?I@Y@y;BP@gxcq{36cJ z&WP{w@_sMV``iC4yUocjhLT#YPkg1fYazEa5Z}J1u1odO)5>QkeEh;K94rbwM za^J3ri;v$gEk*XoP=-eO*D7>h(QX@&c$r=F45&>GWm=AOI zv;cDJx=w!u2%R8?Qx-llI_e}xWT$)9$K3n)c(#_it=0MJ!mAnRFhJ7}H)mla%LDsu zaz6%Ht!~0hXS9?MzwoT)M$B8dcVubwyUG!Ir|1+ zSzy1z)=X9ZjqfE|4zDnT12wKvARX-g9(`8~mWh~F$@fKz2UqTqMYH%dE4?&h zRzgagDU&ePhC7r|*LW=bK>tv9ME@C@CmVhLEiLmioaT=%H$WAvPyi|KFQNY0x7h-A z`TY_irkwS-!@Q^Vg9?xJeO2T_KRslOfAcUt#fFBl*GaP1i6u0nk4L8;u?(uvs^gaF zKnpVuS z<7U@~6OD{obMA+KORK6JoSWuuF$w(VR`Hf1$?SGwT=nv2du*hh{r&jxaFC8jh(SF5 z^^0nY?xU0{H7RPTg$dW{w6B(qtNm&H5%k}J3N1rbxpQjjWw^sQlut{pj4W!5_Ou;~ zzxig35wWs_YVUmd$@bAw0@xwKQrj{sjZO_FTpBZ`7St6T8P~bQ(%o=4Z4F}kDonL~ z)hzgV>DXbcl>}S-d}VS5ndwUQ#(qxE+H&UClJCpg{A9;njVY>8)U=nsa2F01D&jSh zPyLa9sybB2R$PA1U**zBU-LFal)Q>mKW~~!hv-lZ+d~A(mxGe~?9(lltPu}SPefmM z7(ZQlEJZrHkunNjMR_qpdTBs($e9lzL`5unFSXo{W~Ek2Ks2^ko3}6?J~KVN|Fs|* zclbcR*6Jp{EZ4o26is@=R$nh=y3{+$NI-G93aNh%>hq?LO>$g&j8e!vKz^LH9_Y~I z*{m@wHxi**H8gB%iG7>OztY{mJyK)*`{{O&aX--<(xiO$G*Dy5?5yH81&LQR!DsQo zYx6Inyu01$MxvMG?8NVk6jS*yA3r_CfBLGGzcF54X?9Dq!f?IX(r7p#5~RVtlamuO z`$yD9{7q&3%gJABUl6gKTre*ZBB&H^$x}pR-BEkNd+WSl12S!td!r9kMGr zId&%T?`&U+TZVRy3PSqnX}VP1K(3qFo#DlZL%^~GYU|IU3#M3=HJ_mT;;z3WS?4_Q z-}^Ma>@@K-`4iBa1!F@}5YH=XauGmqUJ&f@%o>_7Pg(7`dwn{*U3InfD1IWHuz`1A zyN0)+taQ4<=7qSPzcO95Wn5;_#vtEV?X1*&$Y3e&>xbuePeVAXR?xz9zp*Cic=1At zJ-xjLnf84vpG!MlcO{}J!uQgpKvR~nnT@HGB+|0)@xeMF?Q(Q?bL&`g)K5pF_kUUk zcfcRmoeM%NNrRkYW1@UFWomgz=H}*DLaC%yyqSJuuYkSC0vMk3)4Ns22L*e~^huBX z)`n|bGdZE;;f41&f*?4XVb+qZ$W4Stf#C}^G&FP?$DZ)y-KYo%Wb{!zKVNbMFC_`l z#P!@tQhGll7+Zyh;0%3x$47gw!+BbibUAq8Z^BCOE!BvwJ@V30lM%Pa$tqoF`{!-2eho!Z$}rg7z`_yWIQ;my{6_FZD|JF(o#htRuf1cY|p!YThDiXK)|(?U?vy| zuw{OJeg|Ir!QuF`03HM>rhsd$fnYo=+?OV%(DiKf-8Vj2cqq6m6v?_ZRgIT-by&Q& z7()J^lj`W`2o#*LXafld&=0QuSXJxT4S=Q+_Rz^327q?Z3YdJkE44>7>#^WB)!@Jk z9A=Boji7`c(NM4)@MvxkTj{ZS^$A}VPTB>EvZmx!lQDI#Cy~hwkdIJbmi-#NIBceCEy1M!-g*}(TXayxChzDwXlR4%r z5QN$~kDC+!>F8WGcuH38OSUBoO)$amKL%X;gOV;Vw}A?C$PMy>vN96JUb|$pmmg1> z1sMJ9NA6qVV<7+8a;yGSaCqC&;Z{p$<>27q=IXjQk}oIjyaKat zInvF3=wkJ&odk*=UiW#z0{^YfnL-k+3k+3ZW`ZZtT}%2}#>U$~BjW=OEdTwKJTqX! zIaw75005!dyo(U15mmr-#+l;zJ3m4K#(KM|<==E4ko6g$K}i*`SiV!p!uGsc=A(|* zT6~TzTwSSv|IOEz(Y}GtcY3v^X*%EAN|`F1`aODiCpi6b_7X?;}t+^)JyXx8PO$(Rybs+pKz`qeBfGOW|r+50<8?QTlbSsDZsF{@b^2 zQP%%S0f3wSaM3oU;4OW*f9hN2C$*5;OaqJ~_=QyL>_1(}gIN~C8SFEz7;PV9{XX(u zAX+>M8)F@Cw${>0F@=XlQ56NVsBSn$bOqgiJu$fZx^d3FucrBTEEq%9Xp4ix6x)N_ z)lSPj!NUNHQcDhZS=9zDf5xbcK|A-!-fifrx@*dgQj?UeVkzn)8 zZ5T=~sw^sUWL|dxP%zXeeSiQUG_0@EknOtVVA^rIY4E`;O&yN$WSM|zD=8@0J$drpi&#BZ(jX~x zBQ!A=IAaSyIe*C!0c@+ET{kp4cAw?00~2DlI*@;9=@}TD0M=KKwlc4#>kw5F0So4I zV`F?x&fcP7aq9vCG`Q;M__%AswR8QTWBp(WyIGlaK-Yr+@-$HSZk1@i8r9pRaT8%@ z{c>+$pR;YDt!v4V*Q069ziDo#I5j#Qt-+FI9|X6^ETa1%%7;-YEGW_DDc$sT;GjG; z;h@Ys2(4n= 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_robot_description/docs/index.rst b/bitbots_robot_description/docs/index.rst new file mode 100644 index 00000000..e76aa433 --- /dev/null +++ b/bitbots_robot_description/docs/index.rst @@ -0,0 +1,21 @@ +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_bringup/launch/motion_base.launch b/bitbots_robot_description/launch/load_robot_description.launch similarity index 57% rename from bitbots_bringup/launch/motion_base.launch rename to bitbots_robot_description/launch/load_robot_description.launch index 452663d1..0bffcf86 100644 --- a/bitbots_bringup/launch/motion_base.launch +++ b/bitbots_robot_description/launch/load_robot_description.launch @@ -1,11 +1,9 @@ - - - + diff --git a/bitbots_bringup/launch/move_group.py b/bitbots_robot_description/launch/move_group.py similarity index 98% rename from bitbots_bringup/launch/move_group.py rename to bitbots_robot_description/launch/move_group.py index 55f32b98..10bf65ec 100644 --- a/bitbots_bringup/launch/move_group.py +++ b/bitbots_robot_description/launch/move_group.py @@ -4,7 +4,6 @@ from ament_index_python import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.utilities import normalize_to_list_of_substitutions from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from launch.substitutions import Command, TextSubstitution, LaunchConfiguration @@ -100,7 +99,7 @@ def launch_setup(context, *args, **kwargs): print("### WARNING: trajectory_execution is None") if planning_scene_monitor_parameters is None: print("### WARNING: planning_scene_monitor_parameters is None") - + rsp_node = Node(package='robot_state_publisher', executable='robot_state_publisher', diff --git a/bitbots_robot_description/package.xml b/bitbots_robot_description/package.xml new file mode 100644 index 00000000..282b71c8 --- /dev/null +++ b/bitbots_robot_description/package.xml @@ -0,0 +1,30 @@ + + + + bitbots_robot_description + 2.3.1 + + The bitbots_robot_description package includes utilities to launch the robot description, + robot state publisher and the robot calibration. + + + Florian Vahl + Hamburg Bit-Bots + Hamburg Bit-Bots + MIT + + ament_cmake + + bitbots_docs + bitbots_extrinsic_calibration + moveit_ros_move_group + robot_state_publisher + xacro + + + + unknown + + ament_cmake + + diff --git a/bitbots_bringup/launch/base.launch b/bitbots_utils/launch/parameter_blackboard.launch similarity index 67% rename from bitbots_bringup/launch/base.launch rename to bitbots_utils/launch/parameter_blackboard.launch index af69895c..5545e105 100644 --- a/bitbots_bringup/launch/base.launch +++ b/bitbots_utils/launch/parameter_blackboard.launch @@ -1,7 +1,5 @@ - - diff --git a/bitbots_utils/package.xml b/bitbots_utils/package.xml index 8ba9911d..d2870c30 100644 --- a/bitbots_utils/package.xml +++ b/bitbots_utils/package.xml @@ -17,6 +17,7 @@ bitbots_docs python3-yaml python3-transforms3d + demo_nodes_cpp python3-numpy geometry_msgs ros2_numpy diff --git a/bitbots_utils/scripts/tf_delay_plot.cpp b/bitbots_utils/scripts/tf_delay_plot.cpp index 3df79761..cb9fb6b7 100644 --- a/bitbots_utils/scripts/tf_delay_plot.cpp +++ b/bitbots_utils/scripts/tf_delay_plot.cpp @@ -52,10 +52,3 @@ int main(int argc, char **argv) { rclcpp::shutdown(); return 0; } - - -/// How do I set simtime to true while running a cpp node directly? - -// Answer: - -// ros2 run demo_nodes_cpp talker --ros-args --use-sim-time \ No newline at end of file From f29592b648d833fba6e7ccfa0c6fe0654764b0be Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Tue, 12 Sep 2023 18:38:16 +0200 Subject: [PATCH 04/20] Add multiple orders --- bitbots_utils/bitbots_utils/transforms.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/bitbots_utils/bitbots_utils/transforms.py b/bitbots_utils/bitbots_utils/transforms.py index e4877ad9..81bf825b 100644 --- a/bitbots_utils/bitbots_utils/transforms.py +++ b/bitbots_utils/bitbots_utils/transforms.py @@ -39,8 +39,15 @@ def sixd2quat(sixd): return mat2quat(mat) -def quat2fused(q): - q_xyzw = wxyz2xyzw(q) +def quat2fused(q, order='wxyz'): + # Check quaternion order + if order == 'xyzw': + q_xyzw = q + elif order == 'wxyz': + q_xyzw = wxyz2xyzw(q) + else: + raise ValueError('Unknown quaternion order: {}'.format(order)) + # Fused yaw of Quaternion fused_yaw = 2.0 * math.atan2(q_xyzw[2], q_xyzw[3]) # Output of atan2 is [-tau/2,tau/2], so this expression is in [-tau,tau] @@ -68,7 +75,7 @@ def quat2fused(q): return fused_roll, fused_pitch, fused_yaw, hemi -# Conversion: Fused angles (3D/4D) --> Quaternion +# Conversion: Fused angles (3D/4D) --> Quaternion (wxyz) def fused2quat(fusedRoll, fusedPitch, fusedYaw, hemi): # Precalculate the sine values sth = math.sin(fusedPitch) From ef7a17048e42e827110c4f73e867b2837bc69dcf Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 17 Oct 2023 21:11:22 +0200 Subject: [PATCH 05/20] Remove hlm --- bitbots_bringup/launch/rosbag_record.launch.py | 1 - bitbots_teleop/CMakeLists.txt | 4 ++-- bitbots_teleop/bitbots_teleop/joy_node.py | 11 ++++------- bitbots_teleop/package.xml | 9 ++++----- bitbots_utils/scripts/motor_goals_viz_helper.py | 6 +++--- 5 files changed, 13 insertions(+), 18 deletions(-) diff --git a/bitbots_bringup/launch/rosbag_record.launch.py b/bitbots_bringup/launch/rosbag_record.launch.py index cfa6ad5b..ccf84b64 100644 --- a/bitbots_bringup/launch/rosbag_record.launch.py +++ b/bitbots_bringup/launch/rosbag_record.launch.py @@ -45,7 +45,6 @@ '/joint_states', '/motion_odometry', '/move_base/current_goal', - '/obstacles_relative', '/pose_with_covariance', '/robot_state', '/robots_relative', diff --git a/bitbots_teleop/CMakeLists.txt b/bitbots_teleop/CMakeLists.txt index 5a763820..acd4bf9f 100644 --- a/bitbots_teleop/CMakeLists.txt +++ b/bitbots_teleop/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.5) project(bitbots_teleop) +find_package(ament_cmake REQUIRED) find_package(bitbots_docs REQUIRED) +find_package(bitbots_msgs REQUIRED) find_package(rclpy REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(humanoid_league_msgs REQUIRED) enable_bitbots_docs() diff --git a/bitbots_teleop/bitbots_teleop/joy_node.py b/bitbots_teleop/bitbots_teleop/joy_node.py index ad726a37..1f7c768d 100755 --- a/bitbots_teleop/bitbots_teleop/joy_node.py +++ b/bitbots_teleop/bitbots_teleop/joy_node.py @@ -3,19 +3,16 @@ import threading import rclpy -from humanoid_league_msgs.action import PlayAnimation from rclpy.duration import Duration from rclpy.node import Node from rclpy.action import ActionClient import copy -from pathlib import Path -from sensor_msgs.msg import Joy +from bitbots_msgs.action import PlayAnimation +from bitbots_msgs.msg import Audio +from bitbots_msgs.msg import JointCommand, HeadMode from geometry_msgs.msg import Twist -from humanoid_league_msgs.msg import Audio, HeadMode -from bitbots_msgs.msg import JointCommand - -#from bitbots_animation_server.action import PlayAnimationAction +from sensor_msgs.msg import Joy class JoyNode(Node): diff --git a/bitbots_teleop/package.xml b/bitbots_teleop/package.xml index 90425513..7c5db8eb 100644 --- a/bitbots_teleop/package.xml +++ b/bitbots_teleop/package.xml @@ -12,14 +12,13 @@ MIT ament_cmake - rclpy - humanoid_league_msgs - bitbots_msgs + bitbots_docs + bitbots_msgs + rclpy + std_msgs tf_transformations - std_msgs - python2 diff --git a/bitbots_utils/scripts/motor_goals_viz_helper.py b/bitbots_utils/scripts/motor_goals_viz_helper.py index 762f9f38..53d085c9 100755 --- a/bitbots_utils/scripts/motor_goals_viz_helper.py +++ b/bitbots_utils/scripts/motor_goals_viz_helper.py @@ -4,12 +4,12 @@ import sys import rclpy + +from bitbots_msgs.msg import Animation, JointCommand from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node -from std_msgs.msg import Float64MultiArray from sensor_msgs.msg import JointState -from bitbots_msgs.msg import JointCommand -from humanoid_league_msgs.msg import Animation +from std_msgs.msg import Float64MultiArray class MotorVizHelper(Node): From 05b32dee35701177f960cbb4859474e4c736c4b8 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Fri, 20 Oct 2023 18:11:53 +0200 Subject: [PATCH 06/20] Remove convinience frames --- bitbots_bringup/launch/motion.launch | 5 - bitbots_bringup/package.xml | 1 - bitbots_convenience_frames/CMakeLists.txt | 45 ---- .../docs/_static/logo.png | Bin 11686 -> 0 bytes bitbots_convenience_frames/docs/conf.py | 193 ----------------- bitbots_convenience_frames/docs/index.rst | 21 -- .../convenience_frames.h | 29 --- .../launch/convenience_frames.launch | 14 -- bitbots_convenience_frames/package.xml | 32 --- .../src/convenience_frames.cpp | 198 ------------------ 10 files changed, 538 deletions(-) delete mode 100644 bitbots_convenience_frames/CMakeLists.txt delete mode 100644 bitbots_convenience_frames/docs/_static/logo.png delete mode 100644 bitbots_convenience_frames/docs/conf.py delete mode 100644 bitbots_convenience_frames/docs/index.rst delete mode 100644 bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h delete mode 100644 bitbots_convenience_frames/launch/convenience_frames.launch delete mode 100644 bitbots_convenience_frames/package.xml delete mode 100644 bitbots_convenience_frames/src/convenience_frames.cpp diff --git a/bitbots_bringup/launch/motion.launch b/bitbots_bringup/launch/motion.launch index e52c3a55..d48c6b9a 100644 --- a/bitbots_bringup/launch/motion.launch +++ b/bitbots_bringup/launch/motion.launch @@ -16,11 +16,6 @@ - - - - - diff --git a/bitbots_bringup/package.xml b/bitbots_bringup/package.xml index aed24330..bb4e99a6 100644 --- a/bitbots_bringup/package.xml +++ b/bitbots_bringup/package.xml @@ -18,7 +18,6 @@ audio_common bitbots_basler_camera bitbots_body_behavior - bitbots_convenience_frames bitbots_dynamic_kick bitbots_dynup bitbots_hcm diff --git a/bitbots_convenience_frames/CMakeLists.txt b/bitbots_convenience_frames/CMakeLists.txt deleted file mode 100644 index a9431ae5..00000000 --- a/bitbots_convenience_frames/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(bitbots_convenience_frames) - -# Add support for C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(biped_interfaces REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(bitbots_docs REQUIRED) -find_package(tf2 REQUIRED) -find_package(rclpy REQUIRED) - -include_directories(include) - -add_compile_options(-Wall -Werror -Wno-unused) - -add_executable(convenience_frames src/convenience_frames.cpp) - -ament_target_dependencies(convenience_frames - ament_cmake - bitbots_docs - biped_interfaces - rclcpp - std_msgs - tf2 - tf2_geometry_msgs - tf2_ros -) - -enable_bitbots_docs() - -install(TARGETS convenience_frames - DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/bitbots_convenience_frames/docs/_static/logo.png b/bitbots_convenience_frames/docs/_static/logo.png deleted file mode 100644 index f8afdd5d066232e2dba94b758c6c44fe72b42820..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11686 zcmXw92RN1Q|9;Jm5r>RpWG7@(BpJsDp=`1phm^hd-h0QfqL3Xzj!i~JRv}wfGBUII zKi}VV{om_2=klKSea`bfgp&VAb|Sc&F*NFgB@tx<-Z;YfUD5ico@ud|H^*oUSla|u+Kdso~i3-C87>d;4E{yfHSMIeSYOAHl#@;HngrMIR za(p~|u$?+a2JyvKOCTfT;B1gttk6vv#`gO?`gt~(xpedquJ|CL=|PKn?m9DD=mk^Y z`pvEr56IzTL$yfX%#R0!t^&gc*Om=4+dse|CnvY0%~O{v^gtfA1liK!FWrLd>t?nK z23GeY3%FpxffosI;X?I+nzx%0qG}G}@BNLfK7d%1$HcNQ z7^x>Pa})}-B+n*Pxp@XIxVsR(qwlQ3YSMhQs<^3m)2aPq+MK?l*ksrL8ST zs;06I?Xet3C`*od*ZRSb87}nw`*$TZH8lp$TxlfCiI+zxR+Pcfu z^bsrm_VzY{iAj-*s?#FwyAnt6-k#IAQqWj*6bG+pi&z_s5(nii z>D=|vk&!^nqv67*Z;)Rt_vO;jEt+E?cPupMWMMzwa`g|~)2lq>d(UJ%c~?R-Q&rul zI5l|P%aQTMxTraTkawWJ;U;L+RU)5@n2YZj@g_E&gqHvC54JT}$clI6@mgnrFp{cU zD<>z1!P3Aa^^9*I*c>6Kc8jq$X#KS2kuN`fNXM7HzCP1G*efyWD#-{1tqI0nzo~8a z20ofwfm@Mq1&Rk*Hw(DUe_v5;`x;4!( zoWhx;;6;k~C{99CCnLi`M~Qz`k?C<~hKZTk8V?U|VN!HEH(E!6C~dpYw@~uh5zL?Q zoktde5|NIaq$f~)R7dky`~wjg4GAiReDz|NhlPigDienFKeTmp>gJdCF3N_i zYIz5Q`35MF`6Jl+`UfS+`09#+^j{p;1qYe4TwXW4!P)S*Q@&qO`e8LSqH+_4k6VJl zV8%n?DzN+wR}2|B`9hv5=fI0$t{5;|6)i0-m)(Z;jp!dMn-OHUk@IbB`iN*U^Uo+m zPXN6Pjsw9dYtZ32rLJy;5f$eOMNIdgag)26kP_K#3D?uRs?@MTu-%4(UCFJE(m&mFL@8hlNXs10|83`h z$+%XA+3?o2CV#jS>N!0FvW~s8PevAkOZ?#`Cv`AW)rjgcZ0 zUclAg_I+Z)KN0?Gt{T;wE!PUyqC+8~6e)z&5JyKJcRu4yL{rPC-4qnYo&G`KeIj^b z2A7JLi<>dljF`Z_is2`*q#I#~VckCNx|m&W-)Uv|P>Ok>q$A|L2=(OoQeKp3>90A; zWEP#TodtU3_U_Cz-L7gq`=bs`KFaxKNE!77&qX0>@F5!`S#MrleZ4<-j(W^Yy&c~0 z!AD+_+K2l^_coOzh;|dCh>ddDqoJ2uYsDxAmm3ArRH6pb`MQ3T=}{!~CMj_^t_{*D9|{p zG$PtwQPwv-8=F;ypgi)4H?8>K1UMfR^wM=_|BeL%C zIDZ)O*5++&CA&nN;y}&rT9yM_rYaj#!g7Dwy_c_F%dsWFTTK4?C36^y8H&^jBhy#g zD$2@Ur*jZJ^4JMwfhiS_gUIFP=7wNq&g04gz1x}+@|0wP+S96qLXUUC8UwB`NHfUt z$k(neVSndh?x7Smu@8k1F|4lV+k$z156W)z2OhQKDP3MJERX%>g4&n&7Ekt;BfIA@ zI}u3MlS}{SDoN_a9Zrn%r{6p}t!nctD=QU1KXNOsg0(4-n5?WUF-jy04EMrcn#pU0 zLR-&zZjW60VmFjBrgvG~`;ZHSGRTaBUcq;mDI?*r?gYuYxzsyM94^Y9%-MUkm@qzaCpE0M4I1Cb)>rmD9DO1SljWj9 z>gi2}nz%L1iNTQ=IuzdcCcpT9YeuiEtZXx=oOeZ>%%qAoRa5eK*2S^WC{mscq@t4_ zRsIB3lO#x(t8s6lP3OhgB<;l*WafXli4Y#*W~WAU4JAKwkEzN)*AF0L)g_< ziSceRx%i{khLmLLJvkAlQs}51mI94PGLeHE$&-c)o%ptTD`;GOdfq}%l7c?P`aXG- zqkge?L%MybD99t;k?Y#?}o=ij^tCTQb2PbL}yRR|(~+6uNF@g1g>2y|8>z8g0M zGB}izK86-uJAJo=4p-$h{p~Jxm%1FA<`Tgk&LhVZE%ha_dtRNcFF7~D6MBUnJRr;( z(tZAH@FNIf@!{d&BvN@*nako})>H}?-J7bdhUgLDLG$w_q@<*(ygJj{?q6;Z8QFm) zIQG<~t*;NZo3#M>`L+M~(S9_Ecc8CyaogiYEUhBu=q%29k)T3q~dx=?zC(E_PrU(LnN#Kc4%PqeUHzzSb@^F4OTD4ArA zDcc9x47U~5ervpKZ53QCR>xt!5)_mo%iv5@#pS0pGcd4{JRlV`9MKbmc@5B%AJMjv z3Nl5#J(gn*KGF-&`P@z(0P|fFG4L*mk;=W%Y_gcOfG@S5)H%#7MLK*}I*P1;VZ)Wa zv{X2uS1xdi5mn##TsTt{+thUY2kURPe?(Uhwfwnh%iTxzjyiYFuV)sqL7JGj_a7>9 za)?Y%RVFN(ed!Rl33_@UouRASWOz5oaW^ON&W~yRTU(i|?6yOV_z6*=?Pu*bh!-w) zJc6G2vOSU@*^T%!*u|kt#+OOqrA) zd=n&fWQ=&phU2o(+8Jizi365Qjp@9t;Lujgf(J7kM#W#_z+4a|-{BLMsI6l*UX&?D zcq2&qLXJiE3y&5rq3`K{jKkkSA4&3h=R4XuIgw>NgrRWIxYXrYEE6q$e&YxFBx)VT zURFg|Inw{lp3|aMUVr~D3pg2`$AauoU%x5RQQq5Oq zo1eXpc%b$TSBv4?$C2F@00x$ia6BK6X3j}TQnHMT*@&0UBe&vK9b%pA)jIR{E_Mk` zET9uw>M9n!w_dzFn3)_U&r4{86+OV(Sk1`lKIDlGo>#7U#FW6EsoJ^Z7`X10n0?QJ zxZkx74|<<@uU!-l4WlO`$|(`ao6FoJPs0!zu9BL$6u7-Oa&jUhv%W6o4o+QzVB@E(!d!-?s%=-0rR^x9Flz++1#?(^#A?@AHU+ zK^=>OxHI+WNA*FBu4FY*r5AfV3(JdefpLEI+yp~Ny5 zBs_2^tM*pQM(AyEXp+Obb4l>epFg`v3BLHuAecEV$B9|I9Q)u(gzNZwWbi@jep=_5 z?Sm8FcGHXQ!OgE6zeh&CL}nyV#0tG1w~>FRai1o}CZh-D|6)lB+1FEDUHu5>0*_Vx zT`DG3v&V^%8b3&nDol^6+ooZrB=df**PC&ovt{P4i-ET6Y3v-Ct{}!C>`qE zmcP`^3desVUMJuCB1me~|G{pr%+DzXICC;fs7b?Wf_?x{Fjgu0itw< z4Y&cv9__Sk8Fl)^-1oEi&c~!L%qQvuZ`1P2#CEq+3o762;u2qWm&k#ecoD31$F!H) z;M^6Phh>Z7Y|Y#-Y7|c6SE+3+K@SA3=iQjL9&CZs@UHvVKjeMNttfp}3KCk9tHh-g z)MO+5I-aWZ;eBN~7AJ9~}xgpP%q+F=^f-^ z*q8!OyE&}eeHr(JX|zH3UJpZ97BhW6mRdc#ueIC;_u*;`xgXc2Zx)&;ec=;PPNwSD znMwc^vDxZ~+Ce-BYC9otTv5`E6(Zc4!f9T<-M%wdOHY3|V5&IhGMq1U7b%iSip65d zN=vaZa7}`@BtJitX&{vpsT7pc(La|QBi);f*St)~!jO;Oy?b~2RN^iI&_?fPZf-{& z&fL_T?Fc!}6dcqbKraUe4CgTQ8zzr3O%N02JG z!%PA}DrMF%Q+4Ls+NM@KK4AACOD!D@a;19uD$;=RP>I8N0l>UGLT5m)`x+Mf&%as< zH%jE>1|)b0XUYp_`aXoiE}q)iZ8tD5Fr*o+ia!#@y*%6~|6st?X%1s}u!zNAYC&07H27(nHCopH!P9!&4c#VHrIeKnA_pM3vB_n zP`q+DG8(Bv4fEcc`xY6H<+}nyaL}Oyp0FipUr7{F4Y@!1?DLmzDDF8donU9)XVkVq zfGaprGnyvr=0!zA`WgyD*3AUhQGtG zJ(Z7|NkR6tFp>X(70Ws$H_@K>=i^q|);Fe#1D`ag;}qQnYI1QM=YX8V5PkHhq%=A` zod$r#ofesR3j2npJG!xk;y2=#pPQQg{W!CI6mIT&moiE<^WlB$+{Wd*UJnLZ^ zV*zM!%V1Y*A-1PXZ}fBE+n^1{d&IiCk}ElnIxa_`k z8!KP*?CL`Ot}cd?(dRh$`lL4~N!Ij zxbI31?~81owj;VmM&O_`fwPjxNw=2b;^N;YS_bJ(|F(WEf!H{SK(WfgTN#MMr2ZbC z7jCTTeii=J8*jcM$BY~k?T~oXF+JUm!N}m^B<3CYB->cpSNj6?z2f7gg#F9vN8 zyDVSMEi5xH-GFn=<0CD)WCvM`q*De3- zSwaT_#n_t!`r0}=I;s{c*VC552u6g!+@)cu$`Tq`!edH6v#glRi@J2Motnr zUp8C;VE9;PHH1+`q24`SpKf1VT-0afgaRu!J#LbcUMQxo#&-`69lM*kx{BV8QzWOd zz=Z^RD1mJEjzP@6{FR*@wWWa~_+8V_jpvi8>k1;8KWv`$uJQC9 zToB|9-^dzDPl%~2DV6Ao+}PgkK0R3XE8w!nb7lwYSIlARBaTYYlqL{{o95!RdBRfe zJ|ipCEb-)}+02YV^i1ifY&>bgVsj1aUg;bKJSlrA>|CN$wSo0#K-qp9In?KMUu_S< zqup|Ea_+h*COWh2{((SPvHsJix22`T#n1BWapx)3;{F+AzYaJ54ITxSEAI5GTZUy9 zhT=6M-2{$79pg|viOPzH>jN!$2ST73k#-w z(*E;Oc+<_UjA~LP?yx!g8?5hfJYy)XeyXbXWVl_N?Bx;S2Hha0$-o1T@uc)ugimd%wae4|yw^51OWW?}^GLL3?h~mFVge|B*(%4( zN|+Ai=(xM28JR(*xL^ih@1f2oyTBw}qff@alfb*XCnzYlo%q=|8J50bF)=_Kn=!Trn_n`QMWS zZ^u?=*kD>}=khvXx$D+wpmFd;7nB>qgdj76iK_c*O{YK_dDGj|lP>z#z1l1`zo9`) zLR9pyz#Jha(nB0dl1`|gF)>gxD?D@QyDo%M(&_u}A=b z6@fqmwuRcg=w{1Ne^*pQ7+t{9CJhq)q-nrqou{PKx_pIdqR|4iJpVH;_Ed+?Q+7J^{;1jCuhb=CcqMO5FbpkZ8)y%h$FK~FZs0vw* zAIAHrjei>Muepc`3JRJG?pQpBw6#>cWZ@Yc%FSHSH5|(BvT!B!2}4ZL9UYyaml&%J zth%$PsZ}iBxZ5RR@%X7fw7|dX&xGt#huR8K{j=bjT?oUE_-3&ie0+ReQ2Ma0A{?J# zKQ@uWMqLpg99*H})gU+<&WEG(OKBT$Ks5vLnW}{9#kUH$IyaBz*Y-SHdz`XK+V@mB z5AHTvpQ>_dF(>pgK%$x|hT`#t9f+q{MP8XgG?3%Xc9uXmDwYDNtDJo3@@C*D3h<+a zHylJjODNGMPSph3Te)eQzlW*m0=Bial{!`Pl|lLq8I8*9)3>Fsg5ua6WT_M$LF5h% zGn-zydG`swh^l=koi-#YfB>y5*u-jSq`CqSLrEOw2-sGqs2X5WMlIgdn-dkACr4EQ zEwRg=Q%g%5peb^w(e?tTm@Jxy+G>;8lvl(nE$VB{5e}iqd;??oM$3J-RJf*~d8G{oY^e6BsSJat z5`QuiPwwrB;qb>q`BUIxIY_JHoqL>E+i~;-D$zS6TE)vumgTz z&(oI;J;HG&!(F$RIz~q`J~f5dX}k8nI4qXl{`>cD-W@sx1N{3dvxt(ty}iO4aeNQ+V(nbH`674g>jFvqV5jPRu6?I z!I7%tR9?9b1cCo12z@VKn?fkkyvxffbIIgDa|EkpLs=$cua0Pr2#Oc$zCEo4y?9_h z8~OANzxh5sX5A~(nNbWa4>-T=d@~)W5Ydz2Oo$U=X(3OS)5c(U8IW-xi{^#kAi zdGhx4EQ@kjXkzj|KU!oZnIoeZ#hU>V$$E#GP&-30^{sCOsaVL94#ipc>DT(+;qkGw z^Y4-Tr7U}_O}OVTarHHqTjfN6VRQ*Pg3SAF&$IbU##y5*_YoXP3W3^q4%E&Y9?`ac zGcq`^z}KQDk~r!PU^bFSc~ovVensVq6)yqmmo zX&=VF?oTsKEYyYVd0upa9GGA`hdsg@?-C|rec*o<3CG77mOL2D1i4h8}Fep5{q z$y7~V_u|h{Kfdkh<(%xG1d3)~Yj}e+`zeq#pO3gU%Q-?{Zn#RkFMS0{W>&-WnEF=ra~>G0=z5n| z7r@qWy~|DG8;27%=xO21!I*IK3ntc;KIE|fKmsg&IX9|z8QLYsR$kjW(Gfz@yN5zI2@lWE_)zt|!&S@oQ+`Cv&VAavl(9yXA;?I@Y@y;BP@gxcq{36cJ z&WP{w@_sMV``iC4yUocjhLT#YPkg1fYazEa5Z}J1u1odO)5>QkeEh;K94rbwM za^J3ri;v$gEk*XoP=-eO*D7>h(QX@&c$r=F45&>GWm=AOI zv;cDJx=w!u2%R8?Qx-llI_e}xWT$)9$K3n)c(#_it=0MJ!mAnRFhJ7}H)mla%LDsu zaz6%Ht!~0hXS9?MzwoT)M$B8dcVubwyUG!Ir|1+ zSzy1z)=X9ZjqfE|4zDnT12wKvARX-g9(`8~mWh~F$@fKz2UqTqMYH%dE4?&h zRzgagDU&ePhC7r|*LW=bK>tv9ME@C@CmVhLEiLmioaT=%H$WAvPyi|KFQNY0x7h-A z`TY_irkwS-!@Q^Vg9?xJeO2T_KRslOfAcUt#fFBl*GaP1i6u0nk4L8;u?(uvs^gaF zKnpVuS z<7U@~6OD{obMA+KORK6JoSWuuF$w(VR`Hf1$?SGwT=nv2du*hh{r&jxaFC8jh(SF5 z^^0nY?xU0{H7RPTg$dW{w6B(qtNm&H5%k}J3N1rbxpQjjWw^sQlut{pj4W!5_Ou;~ zzxig35wWs_YVUmd$@bAw0@xwKQrj{sjZO_FTpBZ`7St6T8P~bQ(%o=4Z4F}kDonL~ z)hzgV>DXbcl>}S-d}VS5ndwUQ#(qxE+H&UClJCpg{A9;njVY>8)U=nsa2F01D&jSh zPyLa9sybB2R$PA1U**zBU-LFal)Q>mKW~~!hv-lZ+d~A(mxGe~?9(lltPu}SPefmM z7(ZQlEJZrHkunNjMR_qpdTBs($e9lzL`5unFSXo{W~Ek2Ks2^ko3}6?J~KVN|Fs|* zclbcR*6Jp{EZ4o26is@=R$nh=y3{+$NI-G93aNh%>hq?LO>$g&j8e!vKz^LH9_Y~I z*{m@wHxi**H8gB%iG7>OztY{mJyK)*`{{O&aX--<(xiO$G*Dy5?5yH81&LQR!DsQo zYx6Inyu01$MxvMG?8NVk6jS*yA3r_CfBLGGzcF54X?9Dq!f?IX(r7p#5~RVtlamuO z`$yD9{7q&3%gJABUl6gKTre*ZBB&H^$x}pR-BEkNd+WSl12S!td!r9kMGr zId&%T?`&U+TZVRy3PSqnX}VP1K(3qFo#DlZL%^~GYU|IU3#M3=HJ_mT;;z3WS?4_Q z-}^Ma>@@K-`4iBa1!F@}5YH=XauGmqUJ&f@%o>_7Pg(7`dwn{*U3InfD1IWHuz`1A zyN0)+taQ4<=7qSPzcO95Wn5;_#vtEV?X1*&$Y3e&>xbuePeVAXR?xz9zp*Cic=1At zJ-xjLnf84vpG!MlcO{}J!uQgpKvR~nnT@HGB+|0)@xeMF?Q(Q?bL&`g)K5pF_kUUk zcfcRmoeM%NNrRkYW1@UFWomgz=H}*DLaC%yyqSJuuYkSC0vMk3)4Ns22L*e~^huBX z)`n|bGdZE;;f41&f*?4XVb+qZ$W4Stf#C}^G&FP?$DZ)y-KYo%Wb{!zKVNbMFC_`l z#P!@tQhGll7+Zyh;0%3x$47gw!+BbibUAq8Z^BCOE!BvwJ@V30lM%Pa$tqoF`{!-2eho!Z$}rg7z`_yWIQ;my{6_FZD|JF(o#htRuf1cY|p!YThDiXK)|(?U?vy| zuw{OJeg|Ir!QuF`03HM>rhsd$fnYo=+?OV%(DiKf-8Vj2cqq6m6v?_ZRgIT-by&Q& z7()J^lj`W`2o#*LXafld&=0QuSXJxT4S=Q+_Rz^327q?Z3YdJkE44>7>#^WB)!@Jk z9A=Boji7`c(NM4)@MvxkTj{ZS^$A}VPTB>EvZmx!lQDI#Cy~hwkdIJbmi-#NIBceCEy1M!-g*}(TXayxChzDwXlR4%r z5QN$~kDC+!>F8WGcuH38OSUBoO)$amKL%X;gOV;Vw}A?C$PMy>vN96JUb|$pmmg1> z1sMJ9NA6qVV<7+8a;yGSaCqC&;Z{p$<>27q=IXjQk}oIjyaKat zInvF3=wkJ&odk*=UiW#z0{^YfnL-k+3k+3ZW`ZZtT}%2}#>U$~BjW=OEdTwKJTqX! zIaw75005!dyo(U15mmr-#+l;zJ3m4K#(KM|<==E4ko6g$K}i*`SiV!p!uGsc=A(|* zT6~TzTwSSv|IOEz(Y}GtcY3v^X*%EAN|`F1`aODiCpi6b_7X?;}t+^)JyXx8PO$(Rybs+pKz`qeBfGOW|r+50<8?QTlbSsDZsF{@b^2 zQP%%S0f3wSaM3oU;4OW*f9hN2C$*5;OaqJ~_=QyL>_1(}gIN~C8SFEz7;PV9{XX(u zAX+>M8)F@Cw${>0F@=XlQ56NVsBSn$bOqgiJu$fZx^d3FucrBTEEq%9Xp4ix6x)N_ z)lSPj!NUNHQcDhZS=9zDf5xbcK|A-!-fifrx@*dgQj?UeVkzn)8 zZ5T=~sw^sUWL|dxP%zXeeSiQUG_0@EknOtVVA^rIY4E`;O&yN$WSM|zD=8@0J$drpi&#BZ(jX~x zBQ!A=IAaSyIe*C!0c@+ET{kp4cAw?00~2DlI*@;9=@}TD0M=KKwlc4#>kw5F0So4I zV`F?x&fcP7aq9vCG`Q;M__%AswR8QTWBp(WyIGlaK-Yr+@-$HSZk1@i8r9pRaT8%@ z{c>+$pR;YDt!v4V*Q069ziDo#I5j#Qt-+FI9|X6^ETa1%%7;-YEGW_DDc$sT;GjG; z;h@Ys2(4n= 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_convenience_frames/docs/index.rst b/bitbots_convenience_frames/docs/index.rst deleted file mode 100644 index e76aa433..00000000 --- a/bitbots_convenience_frames/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_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h b/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h deleted file mode 100644 index 36fafc68..00000000 --- a/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -using std::placeholders::_1; - -class ConvenienceFramesBroadcaster : public rclcpp::Node { - public: - ConvenienceFramesBroadcaster(); - void loop(); - private: - std::unique_ptr broadcaster_; - geometry_msgs::msg::TransformStamped tf_{geometry_msgs::msg::TransformStamped()}; - std::unique_ptr - tfBuffer_{std::make_unique(this->get_clock())}; - tf2_ros::TransformListener tfListener_{*tfBuffer_, this}; - std::string base_link_frame_, r_sole_frame_, l_sole_frame_, r_toe_frame_, l_toe_frame_, approach_frame_; - - bool is_left_support{false}; - bool got_support_foot_{false}; - void publishTransform(std::string header_frame_id, std::string child_frame_id, double x, double y, double z); - void supportFootCallback(const biped_interfaces::msg::Phase::SharedPtr msg); -}; diff --git a/bitbots_convenience_frames/launch/convenience_frames.launch b/bitbots_convenience_frames/launch/convenience_frames.launch deleted file mode 100644 index 8eb1a3c5..00000000 --- a/bitbots_convenience_frames/launch/convenience_frames.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file diff --git a/bitbots_convenience_frames/package.xml b/bitbots_convenience_frames/package.xml deleted file mode 100644 index c077fa42..00000000 --- a/bitbots_convenience_frames/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - bitbots_convenience_frames - 2.1.0 - Publishes convenience frames for the Hamburg-BitBots code base. - - Marc Bestmann - Hamburg Bit-Bots - Marc Bestmann - - MIT - - ament_cmake - - bitbots_docs - biped_interfaces - std_msgs - tf2 - tf2_geometry_msgs - tf2_ros - geometry_msgs - - - - c++ - tested_robot - - - ament_cmake - - diff --git a/bitbots_convenience_frames/src/convenience_frames.cpp b/bitbots_convenience_frames/src/convenience_frames.cpp deleted file mode 100644 index 5fd1332a..00000000 --- a/bitbots_convenience_frames/src/convenience_frames.cpp +++ /dev/null @@ -1,198 +0,0 @@ -#include - -ConvenienceFramesBroadcaster::ConvenienceFramesBroadcaster() : Node("convenience_frames") { - - this->declare_parameter("base_link_frame", "base_link"); - this->get_parameter("base_link_frame", base_link_frame_); - this->declare_parameter("r_sole_frame", "r_sole"); - this->get_parameter("r_sole_frame", r_sole_frame_); - this->declare_parameter("l_sole_frame", "l_sole"); - this->get_parameter("l_sole_frame", l_sole_frame_); - this->declare_parameter("r_toe_frame", "r_toe"); - this->get_parameter("r_toe_frame", r_toe_frame_); - this->declare_parameter("l_toe_frame", "l_toe"); - this->get_parameter("l_toe_frame", l_toe_frame_); - this->declare_parameter("approach_frame", "approach_frame"); - this->get_parameter("approach_frame", approach_frame_); - - got_support_foot_ = false; - rclcpp::Subscription::SharedPtr walking_support_foot_subscriber = - this->create_subscription("walk_support_state", - 1, - std::bind(&ConvenienceFramesBroadcaster::supportFootCallback, - this, _1)); - rclcpp::Subscription::SharedPtr dynamic_kick_support_foot_subscriber = - this->create_subscription("dynamic_kick_support_state", - 1, - std::bind(&ConvenienceFramesBroadcaster::supportFootCallback, - this, _1)); -} -void ConvenienceFramesBroadcaster::loop() { - rclcpp::Time last_published_time; - auto node_pointer = this->shared_from_this(); - while (rclcpp::ok()) { - rclcpp::Time startTime = this->get_clock()->now(); - - rclcpp::spin_some(node_pointer); - - geometry_msgs::msg::TransformStamped tf_right, // right foot in baselink frame - tf_left, tf_right_toe, // right toes baselink frame - tf_left_toe, support_foot, // support foot in baselink frame - non_support_foot, non_support_foot_in_support_foot_frame, base_footprint_in_support_foot_frame, - front_foot; // foot that is currently in front of the other, in baselink frame - - try { - // first check if transforms are possible to prevent error spam - if (tfBuffer_->_frameExists(r_sole_frame_) && tfBuffer_->_frameExists(l_sole_frame_) && - tfBuffer_->_frameExists(r_toe_frame_) && tfBuffer_->_frameExists(l_toe_frame_) && - tfBuffer_->canTransform(base_link_frame_, r_sole_frame_, this->now()) && - tfBuffer_->canTransform(base_link_frame_, l_sole_frame_, this->now()) && - tfBuffer_->canTransform(base_link_frame_, r_toe_frame_, this->now()) && - tfBuffer_->canTransform(base_link_frame_, l_toe_frame_, this->now())) { - - tf_right = tfBuffer_->lookupTransform(base_link_frame_, - r_sole_frame_, - this->now(), - rclcpp::Duration::from_nanoseconds(1e9 * 0.1)); - tf_left = tfBuffer_->lookupTransform(base_link_frame_, - l_sole_frame_, - this->now(), - rclcpp::Duration::from_nanoseconds(1e9 * 0.1)); - tf_right_toe = tfBuffer_->lookupTransform(base_link_frame_, - r_toe_frame_, - this->now(), - rclcpp::Duration::from_nanoseconds(1e9 * 0.1)); - tf_left_toe = tfBuffer_->lookupTransform(base_link_frame_, - l_toe_frame_, - this->now(), - rclcpp::Duration::from_nanoseconds(1e9 * 0.1)); - - // compute support foot - if (got_support_foot_) { - if (is_left_support) { - support_foot = tf_left; - non_support_foot = tf_right; - } else { - support_foot = tf_right; - non_support_foot = tf_left; - } - } else { - // check which foot is support foot (which foot is on the ground) - if (tf_right.transform.translation.z < tf_left.transform.translation.z) { - support_foot = tf_right; - non_support_foot = tf_left; - } else { - support_foot = tf_left; - non_support_foot = tf_right; - } - } - - // check with foot is in front - if (tf_right.transform.translation.x < tf_left.transform.translation.x) { - front_foot = tf_left_toe; - } else { - front_foot = tf_right_toe; - } - - // get the position of the non support foot in the support frame, used for computing the barycenter - non_support_foot_in_support_foot_frame = tfBuffer_->lookupTransform(support_foot.child_frame_id, - non_support_foot.child_frame_id, - support_foot.header.stamp, - rclcpp::Duration::from_nanoseconds( - 1e9 * 0.1)); - - geometry_msgs::msg::TransformStamped - support_to_base_link = tfBuffer_->lookupTransform(support_foot.header.frame_id, - support_foot.child_frame_id, - support_foot.header.stamp); - - geometry_msgs::msg::PoseStamped approach_frame; - // x at front foot toes - approach_frame.pose.position.x = front_foot.transform.translation.x; - // y between feet - tf2::Transform center_between_foot; - double y = non_support_foot_in_support_foot_frame.transform.translation.y / 2; - center_between_foot.setOrigin({0.0, y, 0.0}); - center_between_foot.setRotation({0, 0, 0, 1}); - tf2::Transform support_foot_tf; - tf2::fromMsg(support_foot.transform, support_foot_tf); - center_between_foot = support_foot_tf * center_between_foot; - approach_frame.pose.position.y = center_between_foot.getOrigin().y(); - // z at ground leven (support foot height) - approach_frame.pose.position.z = support_foot.transform.translation.z; - - // roll and pitch of support foot - double roll, pitch, yaw; - tf2::Quaternion quat; - fromMsg(support_foot.transform.rotation, quat); - tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); - // yaw of front foot - yaw = tf2::getYaw(front_foot.transform.rotation); - - // pitch and roll from support foot, yaw from base link - tf2::Quaternion rotation; - rotation.setRPY(roll, pitch, yaw); - approach_frame.pose.orientation = tf2::toMsg(rotation); - - // in simulation, the time does not always advance between loop iteration - // in that case, we do not want to republish the transform - rclcpp::Time now = this->now(); - if (now != last_published_time) { - last_published_time = now; - - // set the broadcasted transform to the position and orientation of the base footprint - tf_.header.stamp = now; - tf_.header.frame_id = base_link_frame_; - tf_.child_frame_id = approach_frame_; - tf_.transform.translation.x = approach_frame.pose.position.x; - tf_.transform.translation.y = approach_frame.pose.position.y; - tf_.transform.translation.z = approach_frame.pose.position.z; - tf_.transform.rotation = approach_frame.pose.orientation; - broadcaster_->sendTransform(tf_); - } else { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Missing frames for approach frame, not publishing transform"); - } - } - } catch (...) { - // sleep for a bit to avoid spamming the console - rclcpp::sleep_for(std::chrono::milliseconds(1000)); - continue; - } - this->get_clock()->sleep_until( - startTime + rclcpp::Duration::from_nanoseconds(1e9 / 200)); - } -} - -void ConvenienceFramesBroadcaster::supportFootCallback(const biped_interfaces::msg::Phase::SharedPtr msg) { - got_support_foot_ = true; - is_left_support = (msg->phase == biped_interfaces::msg::Phase::LEFT_STANCE); -} - - -void ConvenienceFramesBroadcaster::publishTransform(std::string header_frame_id, std::string child_frame_id, - double x, double y, double z) { - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = this->now(); - transform.header.frame_id = std::move(header_frame_id); - transform.child_frame_id = std::move(child_frame_id); - transform.transform.translation.x = x; - transform.transform.translation.y = y; - transform.transform.translation.z = z; - transform.transform.rotation.x = 0; - transform.transform.rotation.y = 0; - transform.transform.rotation.z = 0; - transform.transform.rotation.w = 1; - broadcaster_->sendTransform(transform); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - // wait till connection with publishers has been established - // so we do not immediately blast something into the log output - rclcpp::sleep_for(std::chrono::milliseconds(500)); - node->loop(); - rclcpp::shutdown(); -} - From 65b8d0e34a4286ad93e17558653cc94882cc6556 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Fri, 20 Oct 2023 18:12:07 +0200 Subject: [PATCH 07/20] Fit warnings at moveit launch --- .../launch/move_group.py | 41 +++++++++++-------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/bitbots_robot_description/launch/move_group.py b/bitbots_robot_description/launch/move_group.py index 10bf65ec..7b2b1a85 100644 --- a/bitbots_robot_description/launch/move_group.py +++ b/bitbots_robot_description/launch/move_group.py @@ -70,6 +70,8 @@ def launch_setup(context, *args, **kwargs): "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } + sensor_config = load_yaml(f"{robot_type.perform(context)}_moveit_config", "config/sensors_3d.yaml") + trajectory_execution = { "moveit_manage_controllers": True, "trajectory_execution.allowed_execution_duration_scaling": 1.2, @@ -113,23 +115,28 @@ def launch_setup(context, *args, **kwargs): arguments=['--ros-args', '--log-level', 'WARN'] ) - move_group_node = Node(package='moveit_ros_move_group', - executable='move_group', - # output='screen', - # hacky merging dicts - parameters=[{ - 'robot_description': robot_description, - 'robot_description_semantic': robot_description_semantic_config, - 'robot_description_kinematics': kinematics_yaml, - 'publish_robot_description_semantic': True, - 'use_sim_time': sim - }, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, ], - arguments=['--ros-args', '--log-level', 'WARN'] - ) # todo joint limits + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + # output='screen', + # hacky merging dicts + parameters=[ + { + 'robot_description': robot_description, + 'robot_description_semantic': robot_description_semantic_config, + 'robot_description_kinematics': kinematics_yaml, + 'publish_robot_description_semantic': True, + 'use_sim_time': sim, + 'octomap_resolution': 0.01, + }, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + sensor_config + ], + arguments=['--ros-args', '--log-level', 'WARN'] + ) # todo joint limits return [move_group_node, rsp_node] From 24a520f3e2e65fd9a54f0d8c89b845911f9b1751 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Fri, 20 Oct 2023 18:12:36 +0200 Subject: [PATCH 08/20] Use dynup for walkready in teleop --- bitbots_teleop/scripts/teleop_keyboard.py | 162 +++++----------------- 1 file changed, 34 insertions(+), 128 deletions(-) diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py index 585d391c..dc1e48e2 100755 --- a/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_teleop/scripts/teleop_keyboard.py @@ -2,7 +2,6 @@ # This script was based on the teleop_twist_keyboard package # original code can be found at https://github.com/ros-teleop/teleop_twist_keyboard -import math import os import threading @@ -16,97 +15,12 @@ import sys, select, termios, tty from rclpy.action import ActionClient -from bitbots_msgs.action import Kick -from geometry_msgs.msg import Vector3, Quaternion +from bitbots_msgs.action import Kick, Dynup +from geometry_msgs.msg import Vector3 from std_msgs.msg import Bool from std_srvs.srv import Empty -from tf_transformations import quaternion_from_euler -import argparse - -# the following encodes walkready poses for various robots. could be done more nicely via some config - -__walkready_joints__ = { - "wolfgang": - [("HeadPan", 0.0), - ("HeadTilt", 0.0), - ("LShoulderPitch", math.radians(75.27)), - ("LShoulderRoll", 0.0), - ("LElbow", math.radians(35.86)), - ("RShoulderPitch", math.radians(-75.58)), - ("RShoulderRoll", 0.0), - ("RElbow", math.radians(-36.10)), - ("LHipYaw", -0.0112), - ("LHipRoll", 0.0615), - ("LHipPitch", 0.4732), - ("LKnee", 1.0058), - ("LAnklePitch", -0.4512), - ("LAnkleRoll", 0.0625), - ("RHipYaw", 0.0112), - ("RHipRoll", -0.0615), - ("RHipPitch", -0.4732), - ("RKnee", -1.0059), - ("RAnklePitch", 0.4512), - ("RAnkleRoll", -0.0625)], - "robotis_op2": - [("LElbow", math.radians(-60)), - ("RElbow", math.radians(60)), - ("LShoulderPitch", math.radians(120.0)), - ("RShoulderPitch", math.radians(-120.0))], - "op3": - [("l_el", math.radians(-140)), - ("r_el", math.radians(140)), - ("l_sho_pitch", math.radians(-135)), - ("r_sho_pitch", math.radians(135)), - ("l_sho_roll", math.radians(-90)), - ("r_sho_roll", math.radians(90))], - "nao": - [("LShoulderPitch", 1.57), - ("RShoulderPitch", 1.57), - ('LShoulderRoll', 0.3), - ('RShoulderRoll', -0.3)], - "rfc": - [("LeftElbow", math.radians(-90.0)), - ("RightElbow", math.radians(90.0)), - ("LeftShoulderPitch [shoulder]", math.radians(45.0),), - ("RightShoulderPitch [shoulder]", math.radians(-45.0))], - "chape": - [("leftElbowYaw", math.radians(-160)), - ("rightElbowYaw", math.radians(160)), - ("leftShoulderPitch[shoulder]", math.radians(75.27)), - ("rightShoulderPitch[shoulder]", math.radians(75.58)), - ("leftShoulderYaw", math.radians(-75.58)), - ("rightShoulderYaw", math.radians(75.58))], - "mrl_hsl": - [("Shoulder-L [shoulder]", math.radians(60.0)), - ("Shoulder-R [shoulder]", math.radians(-60.0)), - ("UpperArm-L", math.radians(10.0)), - ("UpperArm-R", math.radians(-10.0)), - ("LowerArm-L", math.radians(-135.0)), - ("LowerArm-R", math.radians(135.0))], - "nugus": - [("left_elbow_pitch", math.radians(-120)), - ("right_elbow_pitch", math.radians(-120)), - ("left_shoulder_pitch [shoulder]", math.radians(120)), - ("right_shoulder_pitch [shoulder]", math.radians(120)), - ("left_shoulder_roll", math.radians(20)), - ("right_shoulder_roll", math.radians(-20))], - "sahrv74": - [("left_shoulder_pitch [shoulder]", math.radians(60.0)), - ("right_shoulder_pitch [shoulder]", math.radians(60.0)), - ("left_shoulder_roll", math.radians(10.0)), - ("right_shoulder_roll", math.radians(10.0)), - ("left_elbow", math.radians(-135.0)), - ("right_elbow", math.radians(-135.0))], - "bez": - [("left_arm_motor_0 [shoulder]", math.radians(0)), - ("right_arm_motor_0 [shoulder]", math.radians(0)), - ("left_arm_motor_1", math.radians(170)), - ("right_arm_motor_1", math.radians(170))] -} -__velocity__ = 5.0 -__accelerations__ = -1.0 -__max_currents__ = -1.0 + msg = """ BitBots Teleop @@ -185,29 +99,6 @@ def __init__(self): self.settings = termios.tcgetattr(sys.stdin) - parser = argparse.ArgumentParser() - parser.add_argument('--robot-type', - help="Which robot type is used {wolfgang, op2, op3, nao, rfc, chape, mrl_hsl}", - default="wolfgang") - args, unknown = parser.parse_known_args() - robot_type = args.robot_type - if robot_type not in ["wolfgang", "op2", "op3", "nao", "rfc", "chape", "mrl_hsl"]: - self.get_logger().fatal( - f"Robot type {robot_type} not known. Should be one of [wolfgang, op2, op3, nao, rfc, chape, mrl_hsl]") - exit() - - # generate walkready command - joint_names = [] - joint_positions = [] - for joint_tuple in __walkready_joints__[robot_type]: - joint_names.append(joint_tuple[0]) - joint_positions.append(joint_tuple[1]) - self.walkready = JointCommand( - joint_names=joint_names, - velocities=[__velocity__] * len(joint_names), - accelerations=[__accelerations__] * len(joint_names), - max_currents=[__max_currents__] * len(joint_names), - positions=joint_positions) # Walking Part self.pub = self.create_publisher(Twist, 'cmd_vel', 1) @@ -242,7 +133,6 @@ def __init__(self): self.head_tilt_step = 0.05 self.walk_kick_pub = self.create_publisher(Bool, "kick", 1) - self.joint_pub = self.create_publisher(JointCommand, "DynamixelController/command", 1) self.reset_robot = self.create_client(Empty, "/reset_pose") self.reset_ball = self.create_client(Empty, "/reset_ball") @@ -250,7 +140,16 @@ def __init__(self): print(msg) self.frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/" - self.client = ActionClient(self, Kick, 'dynamic_kick') + + self.dynup_client = ActionClient(self, Dynup, 'dynup') + if not self.dynup_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('Kick action server not available after waiting 5 seconds') + + self.kick_client = ActionClient(self, Kick, 'dynamic_kick') + if not self.kick_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('Kick action server not available after waiting 5 seconds') + + def getKey(self): tty.setraw(sys.stdin.fileno()) @@ -258,6 +157,14 @@ def getKey(self): key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) return key + + def getWalkready(self): + goal = Dynup.Goal() + goal.direction = "walkready" + result: Dynup.Result = self.dynup_client.send_goal(goal).result + if not result.successful: + self.get_logger().error("Could not execute walkready animation") + return result.successful def generate_kick_goal(self, x, y, direction): kick_goal = Kick.Goal() @@ -297,40 +204,40 @@ def loop(self): self.head_pub.publish(self.head_msg) elif key == 'y': # kick left forward - self.client.send_goal_async(self.generate_kick_goal(0.2, 0.1, 0)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0.1, 0)) elif key == '<': # kick left side ball left - self.client.send_goal_async(self.generate_kick_goal(0.2, 0.1, -1.57)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0.1, -1.57)) elif key == '>': # kick left side ball center - self.client.send_goal_async(self.generate_kick_goal(0.2, 0, -1.57)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, -1.57)) elif key == 'c': # kick right forward - self.client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 0)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 0)) elif key == 'v': # kick right side ball right - self.client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 1.57)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 1.57)) elif key == 'V': # kick right side ball center - self.client.send_goal_async(self.generate_kick_goal(0.2, 0, 1.57)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, 1.57)) elif key == "x": # kick center forward - self.client.send_goal_async(self.generate_kick_goal(0.2, 0, 0)) + self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, 0)) elif key == "X": # kick center backwards - self.client.send_goal_async(self.generate_kick_goal(-0.2, 0, 0)) + self.kick_client.send_goal_async(self.generate_kick_goal(-0.2, 0, 0)) elif key == "b": # kick left backwards - self.client.send_goal_async(self.generate_kick_goal(-0.2, 0.1, 0)) + self.kick_client.send_goal_async(self.generate_kick_goal(-0.2, 0.1, 0)) elif key == "n": # kick right backwards - self.client.send_goal_async(self.generate_kick_goal(-0.2, -0.1, 0)) + self.kick_client.send_goal_async(self.generate_kick_goal(-0.2, -0.1, 0)) elif key == "B": # kick left backwards - self.client.send_goal_async(self.generate_kick_goal(0, 0.14, -1.57)) + self.kick_client.send_goal_async(self.generate_kick_goal(0, 0.14, -1.57)) elif key == "N": # kick right backwards - self.client.send_goal_async(self.generate_kick_goal(0, -0.14, 1.57)) + self.kick_client.send_goal_async(self.generate_kick_goal(0, -0.14, 1.57)) elif key == 'Y': # kick left walk self.walk_kick_pub.publish(Bool(data=False)) @@ -339,8 +246,7 @@ def loop(self): self.walk_kick_pub.publish(Bool(data=True)) elif key == 'F': # play walkready animation - self.walkready.header.stamp = self.get_clock().now().to_msg() - self.joint_pub.publish(self.walkready) + self.getWalkready() elif key == 'r': # reset robot in sim try: From ac14bd85c7a3e8815a122759cf991c99df5baba8 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Fri, 20 Oct 2023 18:12:49 +0200 Subject: [PATCH 09/20] Fix yaw axis --- bitbots_utils/bitbots_utils/transforms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_utils/bitbots_utils/transforms.py b/bitbots_utils/bitbots_utils/transforms.py index 81bf825b..24a42c75 100644 --- a/bitbots_utils/bitbots_utils/transforms.py +++ b/bitbots_utils/bitbots_utils/transforms.py @@ -125,4 +125,4 @@ def compute_imu_orientation_from_world(robot_quat_in_world): def quat_from_yaw(yaw: float) -> Quaternion: - return msgify(Quaternion, wxyz2xyzw(euler2quat(0, 0, yaw, axes='szxy'))) + return msgify(Quaternion, wxyz2xyzw(euler2quat(yaw, 0, 0, axes='szxy'))) From 197b81718ccdfcf52ba1012ffc7c155e024151ef Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Fri, 20 Oct 2023 18:13:01 +0200 Subject: [PATCH 10/20] Fix imu frame name --- bitbots_utils/scripts/dummy_imu.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_utils/scripts/dummy_imu.py b/bitbots_utils/scripts/dummy_imu.py index e86075a5..0151082a 100755 --- a/bitbots_utils/scripts/dummy_imu.py +++ b/bitbots_utils/scripts/dummy_imu.py @@ -11,7 +11,7 @@ def __init__(self): super().__init__('DummyImu') self.pub = self.create_publisher(Imu, '/imu/data', 1) self.msg = Imu() - self.msg.header.frame_id = 'imu' + self.msg.header.frame_id = 'imu_frame' self.msg.orientation.w = 1.0 def loop(self): From 1b56f9b930654f7d002234490c07236ac5904d88 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 23 Oct 2023 09:11:08 +0200 Subject: [PATCH 11/20] Update bitbots_teleop/scripts/teleop_keyboard.py Co-authored-by: Jan Gutsche <34797331+jaagut@users.noreply.github.com> --- bitbots_teleop/scripts/teleop_keyboard.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py index dc1e48e2..e1769920 100755 --- a/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_teleop/scripts/teleop_keyboard.py @@ -143,7 +143,7 @@ def __init__(self): self.dynup_client = ActionClient(self, Dynup, 'dynup') if not self.dynup_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('Kick action server not available after waiting 5 seconds') + self.get_logger().error('Dynup action server not available after waiting 5 seconds') self.kick_client = ActionClient(self, Kick, 'dynamic_kick') if not self.kick_client.wait_for_server(timeout_sec=5.0): From 2053a0f4bb8e3af1e00eb9db8430f70949c8b430 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 23 Oct 2023 09:15:00 +0200 Subject: [PATCH 12/20] Remove duplicate loading of the robot description --- bitbots_bringup/launch/motion.launch | 5 ----- bitbots_bringup/launch/teamplayer.launch | 5 +++++ 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/bitbots_bringup/launch/motion.launch b/bitbots_bringup/launch/motion.launch index d48c6b9a..75cf5f94 100644 --- a/bitbots_bringup/launch/motion.launch +++ b/bitbots_bringup/launch/motion.launch @@ -5,11 +5,6 @@ - - - - - diff --git a/bitbots_bringup/launch/teamplayer.launch b/bitbots_bringup/launch/teamplayer.launch index 899784c3..c6525172 100644 --- a/bitbots_bringup/launch/teamplayer.launch +++ b/bitbots_bringup/launch/teamplayer.launch @@ -19,6 +19,11 @@ + + + + + From 6d11ebb4c43104d1d66d2e922521c8ca30252915 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 23 Oct 2023 11:54:45 +0200 Subject: [PATCH 13/20] Show all node output --- bitbots_robot_description/launch/move_group.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_robot_description/launch/move_group.py b/bitbots_robot_description/launch/move_group.py index 7b2b1a85..995a677e 100644 --- a/bitbots_robot_description/launch/move_group.py +++ b/bitbots_robot_description/launch/move_group.py @@ -106,7 +106,7 @@ def launch_setup(context, *args, **kwargs): rsp_node = Node(package='robot_state_publisher', executable='robot_state_publisher', respawn=True, - # output='screen', + output='screen', parameters=[{ 'robot_description': robot_description, 'publish_frequency': 100.0, @@ -118,7 +118,7 @@ def launch_setup(context, *args, **kwargs): move_group_node = Node( package='moveit_ros_move_group', executable='move_group', - # output='screen', + output='screen', # hacky merging dicts parameters=[ { From f2d0296de951d95be4435fde56994cc174dff5bd Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 23 Oct 2023 11:55:06 +0200 Subject: [PATCH 14/20] Add welcome bit-bot --- bitbots_utils/config/welcome_art.txt | 29 +++++++++++++++++++ .../launch/parameter_blackboard.launch | 4 +++ 2 files changed, 33 insertions(+) create mode 100644 bitbots_utils/config/welcome_art.txt diff --git a/bitbots_utils/config/welcome_art.txt b/bitbots_utils/config/welcome_art.txt new file mode 100644 index 00000000..de18e86b --- /dev/null +++ b/bitbots_utils/config/welcome_art.txt @@ -0,0 +1,29 @@ + +                                                    +                 &&  &&&&&&&&,  &&                  +             &&&&&&&&&&&&&&&&&&&&&&&&(              +           /&&&&&&&&&&&&&&&&&&&&%&&&&&&             +          &&&&&&&&&&&&######%&&&&&&&&&&&            +      &###&&&&&&%&&&&&&&&%&&&&&&%&&&&&&&%##%#       +   &&%###%&&/******(&&&&&&&&&&(*******(&&##%%#&     +  &&&%%&&&/***#%%(***#&&&&&&&***(&&&(***%&&%%&&&    +  &&&&&&&(**%&&&&&&/**&&%&%&/**&&&&&&&**/&&&&&&&&   +  &&&&&&&%**(&&&&&&***&&%%%&#**#&&&&&(**(&&&&&&&#   + #%&&&&&&&&/********(&&&&&&&&&/********&&&&&&&&&%&  + #&&&&&#,&&&&&&%%&&&&&&&&&&&&&&&&&&&&&&&&#%%&&&&&&  + &%%&%#&  (&&&&&&&&&&&&&&&&&&&&&&&&&&&&&  .%%&&%%,  + %%%&.      /&&&%&&&&&&&&&&&&&&&%&&&&&       &%%#&  + %%%%&         *&&&&&&&&&&&&&&&&&&&         .&%%%&  + &%%%%.        *&%%%&& &&&&&&,&&%%%&&         &%%%%  + &%%%&         &&#,*%&        &%(,#&&          (%%%  + &%%&          &&&&&&&        &&&&&&&           %%%  + &%*&&         &&&&&&&        &&&&&&&          &&&%  +              &&&&&&&        &&&%&&&                +             /&&&&&&&&      %&&&&&&&(               +             /&&%&&&&&      %&&&%&&&(               +             /&&&&&&&&      %&&&&&&&(               +             /&&&&&&&&      %&&&%&&&(               +             /&&&&&&&&      &&&&&&&&(               +            //%(#*(%&        &%(*((%/%              +            #///////%        #///////(              +                                                    diff --git a/bitbots_utils/launch/parameter_blackboard.launch b/bitbots_utils/launch/parameter_blackboard.launch index 5545e105..9e404b82 100644 --- a/bitbots_utils/launch/parameter_blackboard.launch +++ b/bitbots_utils/launch/parameter_blackboard.launch @@ -1,6 +1,10 @@ + + + + From 8645b761bf1e03480e2898b4bc797e9e55737ca3 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 23 Oct 2023 14:03:41 +0200 Subject: [PATCH 15/20] Fix bug with kick command --- bitbots_teleop/scripts/teleop_keyboard.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py index e1769920..51efd27f 100755 --- a/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_teleop/scripts/teleop_keyboard.py @@ -16,7 +16,7 @@ import sys, select, termios, tty from rclpy.action import ActionClient from bitbots_msgs.action import Kick, Dynup -from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import Point, Vector3 from std_msgs.msg import Bool from std_srvs.srv import Empty @@ -170,7 +170,7 @@ def generate_kick_goal(self, x, y, direction): kick_goal = Kick.Goal() kick_goal.header.stamp = self.get_clock().now().to_msg() kick_goal.header.frame_id = self.frame_prefix + "base_footprint" - kick_goal.ball_position = Vector3(x=float(x), y=float(y), z=0.0) + kick_goal.ball_position = Point(x=float(x), y=float(y), z=0.0) kick_goal.kick_direction = quat_from_yaw(direction) kick_goal.kick_speed = 1.0 return kick_goal From 4ebde62e824390f7a6351f7ec0eff27ec1a97217 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Mon, 23 Oct 2023 19:00:48 +0200 Subject: [PATCH 16/20] Ad cpp utility function that waits for a number of tf frames --- bitbots_utils/CMakeLists.txt | 47 +++++++++-- bitbots_utils/include/bitbots_utils/utils.hpp | 40 ++++++++++ bitbots_utils/src/utils.cpp | 80 +++++++++++++++++++ 3 files changed, 159 insertions(+), 8 deletions(-) create mode 100644 bitbots_utils/include/bitbots_utils/utils.hpp create mode 100644 bitbots_utils/src/utils.cpp diff --git a/bitbots_utils/CMakeLists.txt b/bitbots_utils/CMakeLists.txt index c9534c28..0e65f4c3 100644 --- a/bitbots_utils/CMakeLists.txt +++ b/bitbots_utils/CMakeLists.txt @@ -1,35 +1,57 @@ cmake_minimum_required(VERSION 3.5) project(bitbots_utils) +# Add support for C++17 +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + find_package(bitbots_docs REQUIRED) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +enable_bitbots_docs() +# Define include directories for cpp library headers set(INCLUDE_DIRS include) include_directories(${INCLUDE_DIRS}) -set(CMAKE_CXX_STANDARD 17) add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra) -enable_bitbots_docs() +# Add cpp library +add_library(${PROJECT_NAME} src/utils.cpp) -add_executable(tf_delay_plot scripts/tf_delay_plot.cpp) +# Add dependencies to cpp library +ament_target_dependencies(${PROJECT_NAME} + rclcpp + tf2_ros) -ament_target_dependencies(tf_delay_plot - ament_cmake - rclcpp - tf2_msgs - std_msgs) +ament_export_dependencies(rclcpp) +ament_export_include_directories(${INCLUDE_DIRS}) +ament_export_libraries(${PROJECT_NAME}) +# Python install ament_python_install_package(${PROJECT_NAME}) +# CPP Script install +add_executable(tf_delay_plot scripts/tf_delay_plot.cpp) + +ament_target_dependencies(tf_delay_plot + ament_cmake + rclcpp + tf2_msgs + std_msgs) + install(TARGETS tf_delay_plot DESTINATION lib/${PROJECT_NAME}) + + +# Copy config and launch files install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) @@ -40,4 +62,13 @@ install(DIRECTORY scripts/ USE_SOURCE_PERMISSIONS DESTINATION lib/${PROJECT_NAME}) +# Install library +install(DIRECTORY include/ + DESTINATION include) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + INCLUDES DESTINATION include) + ament_package() diff --git a/bitbots_utils/include/bitbots_utils/utils.hpp b/bitbots_utils/include/bitbots_utils/utils.hpp new file mode 100644 index 00000000..6d95a148 --- /dev/null +++ b/bitbots_utils/include/bitbots_utils/utils.hpp @@ -0,0 +1,40 @@ +#ifndef BITBOTS_UTILS__UTILS_H_ +#define BITBOTS_UTILS__UTILS_H_ + +#include + +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace bitbots_utils +{ + +/** + * @brief Waits for the transforms to be available + * @param logger The logger to use for logging + * @param clock The clock to use for time + * @param tf_buffer The tf buffer to use + * @param frames The tf frames to wait for + * @param check_interval Interval in which to check for the frames + * @param warn_duration Duration after which to warn if the frames are not available + * @param warn_interval Interval in which to keep warning if the frames are not available + * @param verbose Can be used to disable the warning messages + */ +void wait_for_tf( + const rclcpp::Logger &logger, + std::shared_ptr clock, + std::shared_ptr tf_buffer, + const std::vector &frames, + const std::string &root_frame, + const rclcpp::Duration &check_interval = rclcpp::Duration(0.1s), + const rclcpp::Duration &warn_duration = rclcpp::Duration(5.0s), + const rclcpp::Duration &warn_interval = rclcpp::Duration(1.0s), + bool verbose = true); + +} // namespace bitbots_utils + +#endif // BITBOTS_UTILS__UTILS_H_ diff --git a/bitbots_utils/src/utils.cpp b/bitbots_utils/src/utils.cpp new file mode 100644 index 00000000..c13dbeac --- /dev/null +++ b/bitbots_utils/src/utils.cpp @@ -0,0 +1,80 @@ +#include "bitbots_utils/utils.hpp" + +namespace bitbots_utils +{ + +/** + * @brief Waits for the transforms to be available + * @param logger The logger to use for logging + * @param clock The clock to use for time + * @param tf_buffer The tf buffer to use + * @param frames The tf frames to wait for + * @param check_interval Interval in which to check for the frames + * @param warn_duration Duration after which to warn if the frames are not available + * @param warn_interval Interval in which to keep warning if the frames are not available + * @param verbose Can be used to disable the warning messages + */ +void wait_for_tf( + const rclcpp::Logger &logger, + std::shared_ptr clock, + std::shared_ptr tf_buffer, + const std::vector &frames, + const std::string &root_frame, + const rclcpp::Duration &check_interval, + const rclcpp::Duration &warn_duration, + const rclcpp::Duration &warn_interval, + bool verbose) { + + // Store the beginning time + auto start_time = clock->now(); + + // Endless loop with increasing counter + while(rclcpp::ok()) { + try { + // Check if the frame we want to transform is known yet + // Apply tf_buffer->_frameExists to all frames and check if all are available otherwise return false (functional) + // We use _frameExists because it does not spam the console with errors if the frame does not exist... + if(!std::all_of(frames.begin(), frames.end(), [tf_buffer](std::string frame) {return tf_buffer->_frameExists(frame);})) { + // Don't do busy waiting + rclcpp::sleep_for(check_interval.to_chrono()); + // Retry + continue; + } + + // Check if we can transform from base_link to all given frames + if(!std::all_of( + frames.begin(), + frames.end(), + [tf_buffer, root_frame, check_interval](const std::string frame) { + return tf_buffer->canTransform( + root_frame, + frame, + rclcpp::Time(0), + check_interval); + })){ + // Here it is fine not to wait as the canTransform function already includes a timeout + // Retry + continue; + } + // We can transform to all frames, so we are done + return; + } + catch(const std::exception& e) + { + if (verbose) { + RCLCPP_ERROR(logger, "Error while waiting for transforms: %s \n", e.what()); + } + rclcpp::sleep_for(check_interval.to_chrono()); + } + + // Print error message if we waited too long + auto wait_time = clock->now() - start_time; + if (verbose && wait_time > warn_duration) { + RCLCPP_WARN_THROTTLE(logger, *clock, warn_interval.seconds(), + "Waiting for transforms took longer than %f seconds", + wait_time.seconds()); + } + } +} + +} From b2b8219333bdafa68c0cbd141b508e2c0a5add16 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Wed, 25 Oct 2023 21:45:02 +0200 Subject: [PATCH 17/20] Remove weird readme --- bitbots_bringup/README | 377 ----------------------------------------- 1 file changed, 377 deletions(-) delete mode 100644 bitbots_bringup/README diff --git a/bitbots_bringup/README b/bitbots_bringup/README deleted file mode 100644 index 2a1b778c..00000000 --- a/bitbots_bringup/README +++ /dev/null @@ -1,377 +0,0 @@ -pylon 5 Camera Software Suite for Linux for Use with Basler -Gigabit Ethernet(GigE) and Basler USB 3.0 Cameras (U3V) -================================================================ - -Note: -On some embedded platforms, the pylon Viewer and the IP Configurator -applications may not be available. - -System Requirements -=================== - -GigE ----- -A GigE network adapter that supports jumbo frames is recommended. Concerning -performance and reliability we made best experiences with the Intel PRO 1000, -I210, I340 and I350 series. -Although the pylon software will work with any GigE network adapter, we would -recommend to use one of these adapters. - -USB ---- -For U3V devices a USB3-capable USB controller is necessary. For best -performance and stability we highly recommend a kernel version >= 3.13.x - - -Installation -============= - -The installation of pylon for Linux is described in the INSTALL text document. - - - -Performance Optimization -======================== - -To increase performance and to minimize CPU usage when grabbing images, the -following settings should be considered: - -GigE Devices ------------- -* Enable Jumbo Frames. - Many GigE network adapters support so-called jumbo frames, i.e., network - packets larger than the usual 1500 bytes. To enable jumbo frames, the maximum - transfer unit (MTU) size of the PC's network adapter must be set to a high - value (see the description in the INSTALL document). We recommend using a - value of 8192. - -* Increase the packet size. - When jumbo frames are enabled, the camera's packet size must be increased to - benefit from the larger packets. The 'Optimizing Packet Size' section of the - INSTALL document describes how to set the packet size. - -* Real-time Priority - The GigE Vision implementation of Basler pylon software uses a thread for - receiving image data. Basler pylon tries to set the thread priority for the - receive thread to real-time thread priority. This requires certain - permissions. The 'Permissions for Real-time Thread Priorities' section of the - INSTALL document describes how to grant the required permissions. - -U3V Devices ------------ -* Increasing Packet Size - For faster USB transfers you should increase the packet size. You can do this - by changing the "Stream Parameters" -> "Maximum Transfer Size" value from - inside the pylon Viewer or by setting the corresponding value via the API. - After increasing the package size you will likely run out of kernel space - and see corresponding error messages on the console. The default value set - by the kernel is 16 MB. To set the value (in this example to 1000 MB) you can - execute as root: - echo 1000 > /sys/module/usbcore/parameters/usbfs_memory_mb - This would assign a maximum of 1000 MB to the USB stack. - - -Documentation -============= - -The installation includes a "share/doc/C++" sub-folder containing the -pylon Programmer's Guide and API reference documentation. Open the -index.html file with an internet browser. - - -Sample Programs -=============== - -The installation archive includes a set of sample programs. These sample -programs are simple command line programs showing the basic pylon use cases. -They are located in the "Samples/C++" and "Samples/C" folder. Each folder contains -a top-level Makefile that can be used to build the different sample programs. - -* C++ samples: - If you installed pylon in /opt/pylon5, run: - cd Samples/C++ - make - ./Grab/Grab - - If pylon is installed in a non-standard location, an additional step is required: - cd Samples/C++ - source /bin/pylon-setup-env.sh - make - ./Grab/Grab - -* C samples: - If you installed pylon in /opt/pylon5, run: - cd Samples/C - make - ./SimpleGrab/SimpleGrab - - If pylon is installed in a non-standard location, an additional step is required: - cd Samples/C - source /bin/pylon-setup-env.sh - make - ./SimpleGrab/SimpleGrab - - -Environment Variables -===================== - -From pylon 5.0 upwards, no additional environment variables are required to -run pylon-based applications. - -For development, though, the compiler must know where pylon is installed. -The pylon samples use the environment variable PYLON_ROOT to find the relevant information. - -For your convenience, we created the pylon-setup-env.sh script located in the -pylon5/bin directory which can carry out the complete setup. - -To setup the environment for a pylon installation in execute: - source /bin/pylon-setup-env.sh - -If you want the environment for the standard installation to be persistent, you -can add - source /path/to/your/pylon5/bin/pylon-setup-env.sh /path/to/your/pylon5 -to ~/.bashrc - -For special use cases, you can do a manual environment setup as follows: - - export PYLON_ROOT=/path/to/your/pylon5 - -Even though there is currently only one variable needed, using pylon-setup-env.sh -is still the preferred way to set up your environment, as we might add more variables -in the future. - - -Camera Emulator -=============== - -In addition to camera transport layers like GigE Vision or USB3 Vision, pylon offers -a transport layer that can create simple camera emulation devices. This allows you to -develop applications without the need for a physical camera. It is also useful if you -want to develop a multi-camera application and don't have enough cameras at hand. -Besides emulating image acquisition and standard camera features, camera emulation -devices also offer features that a physical camera does not offer: -- You can display custom test images, e.g., to optimize your image processing algorithms. -- You can generate failed buffers, e.g., to test your exception handling routines. - -The number of available emulator devices can be controlled by exporting the -PYLON_CAMEMU environment variable. For example, - - export PYLON_CAMEMU=2 - -will provide two emulator devices. These devices are accessible both by using -the pylon API and the pylon Viewer program. - -When PYLON_CAMEMU is not set, no emulator devices are provided. -Note: A maximum of 256 emulator devices are supported. - -For more information on the camera emulator refer to the Basler Product Documentation, -which is included in this package (press F1 in the pylon Viewer). - - -GenTL producers -=============== - -In addition to the pylon 5 Camera Software Suite for Linux, this release contains -GenTL 1.5 compliant producers for USB3 Vision and GigE Vision standards. - -To use the Basler GenTL producers you need to setup your environment as follows - - export GENICAM_GENTL32_PATH=/path/to/your/pylon5/lib/gentlproducer/gtl - export GENICAM_GENTL64_PATH=/path/to/your/pylon5/lib64/gentlproducer/gtl - - -Troubleshooting -=============== - -* Problem: When trying to grab frames from multiple cameras, the error message - "PrepareGrab (StartStreaming) failed for device" or "Insufficient system - resources exist to complete the API" are displayed. - - Solution: This condition can occur when the number of available open file - descriptors is exhausted. The limit can often be raised for the current - process with the command: ulimit -n - - -GigE Devices ------------- -* Problem: I can't see my camera in the pylon Viewer, even after waiting for - more than one minute. (Note: Depending on the camera's and adapter's - IP configuration it can take up to one minute until a valid IP address is - assigned to the camera.) - - Solution: Start the Basler IP Configurator (/opt/pylon5/bin/IpConfigurator). - Is the camera shown by the IP Configurator? - If "yes", the camera's IP address is in a different subnet than the network - adapter's IP address. Both, the camera and the network adapter, must use IP - addresses within the same subnet. Refer to the INSTALL document for the IP - address setup. - - If "no", the most likely reason is an enabled firewall. As described in the - INSTALL document, disable the firewall for those network adapters cameras - will be connected to. If you don't have a firewall, enabled reverse-path - filtering in the kernel may prevent detection of the camera. Refer to the - next problem description for further details. - -* Problem: The IP Configurator can't see my camera. I'm not able to - reconfigure the camera to make it visible again. - - Solution: First make sure you don't have a firewall enabled on your network - interface the camera is connected to. - If you still can't see the camera, reverse path filtering in the kernel may - prevent the IP Configurator to detect the camera. On some Linux - distributions reverse path filtering may prevent the discovery of GigE Vision - cameras. This can happen if the camera's IP address is not within the same - subnet as the network adapter the camera is attached to. Normally the IP - Configurator can handle this by using broadcasts to discover the camera on - any subnet. Reverse-path filtering may prevent the IP Configurator to receive - the answer from the broadcast which in turn prevents the IP Configurator - from detecting the camera. To check whether filtering is turned on, run the - following command: - sysctl -a 2>/dev/null | grep '\.rp_filter' - - in the output look for the following lines: - net.ipv4.conf.all.rp_filter=1 - net.ipv4.conf.eth1.rp_filter=1 - - where "eth1" is the network adapter the camera is connected to. - The "net.ipv4.conf.all.rp_filter" is a global switch which must be turned off. - The "net.ipv4.conf.eth1.rp_filter" tells whether filtering for the specified - network adapter is activated. To disable filtering, you must first turn off - filtering for "all" and the specific network interface (in this - sample "eth1"). Use the following commands to change the filtering behavior - at runtime: - sudo sysctl net.ipv4.conf.all.rp_filter=0 - sudo sysctl net.ipv4.conf.eth1.rp_filter=0 - - Restart the IP Configurator and check whether the camera(s) are - detected. Reconfigure the camera(s) and use "Write Configuration" to make - your changes persistent. You can re-enable filtering by executing the same - commands but set a value of 1. If you want to turn off filtering permanently, - you can edit the same values in /etc/sysctl.conf. - - From Linux kernel version 2.6.32 onwards, the rp_filter settings allow strict - and loose filtering. To accept asymmetrically routed packets, modify /etc/sysctl.conf: - net.ipv4.conf.default.rp_filter = 2 - net.ipv4.conf.all.rp_filter = 2 - -* Problem: When grabbing images, the CPU load is higher than expected. - - Solution: Ensure that jumbo frames are enabled and large network packets are - used as described above in the 'Performance Optimization' section. - - -* Problem: Grabbing images leads to errors with error code 0x81010014. - This error code indicates that the PC received incomplete images, i.e., - network packets have been dropped. - - Solution 1: Ensure that jumbo frames are enabled as described above in the - 'Performance Optimization' section. Also make sure that the camera's packet - size is set to a high value, if possible 8192. - - Solution 2: Increase the maximum UDP receive buffer size to a value that is - large enough, e.g. by issuing the sudo sysctl net.core.rmem_max=2097152 - command. This allows pylon to increase the socket buffer size to 2 MB to - ensure a stable image acquisition. To make this setting persistent, you can - add the net.core.rmem_max setting to the /etc/sysctl.conf file. - - Solution 3: Ensure that the application has the required permissions to set - the pylon receive thread's priority to real-time thread priority as described - in the INSTALL document. - - Solution 4: Check the cable. For GigE, Cat 5e cables are recommended. Poor - cable quality or damaged cables can lead to unrecoverable transmission errors. - - Solution 5: The amount of data produced by the camera(s) exceeds the amount - of bandwidth the network adapter(s) or the PC can provide. Reduce the camera - frame rate by increasing the camera's "Inter Packet Delay" parameter. When - using the pylon API, the inter packet delay is controlled via the GevSCPD - parameter. - - -* Problem: The pylon Viewer seems to acquire images but no images are displayed. - - Solution 1: Start the pylon Viewer from the command line to see if the viewer - prints out any messages. - - Solution 2: The viewer only receives incomplete frames. In that case, error - messages such as "Failed to grab image: GX status 0x81010014" are displayed. - Please refer to the previous problem for tips about how to solve this issue. - - Solution 3: No error messages are printed out. All data packets transmitted - from the camera seem to be discarded. Make sure that the camera's current - packet size doesn't exceed the network adapter's MTU size. - - If the packet size is less than or equal to the MTU size, but greater than - 1500, stop image acquisition and set the camera's packet size to 1500. - Restart image acquisition. If an image is displayed using this packet size, - either jumbo frames are not enabled on the network adapter or the adapter - doesn't support jumbo frames. - - Solution 3: Grabbing images is failing for other reasons. Please report the - error messages printed out by the viewer to Basler technical support. - - -U3V Devices ------------ -* Problem: The camera is not found during enumeration. - Solution: Check the user permissions on the device. You must have read/write - permissions. See the output of - ls -lR /dev/bus/usb - to check if you have read/write permissions. To setup the correct udev rules, - execute: - ./setup-usb.sh - from within the directory containing this README. - - -Known Issues -============ - -* Same IP address for camera and network interface. - If the camera has been assigned a permanent IP address that equals the address - assigned to the network adapter, the camera will not be accessible, even when - using the IP Configurator. - - Either change the network adapter's address or use a different PC to assign a - different IP address to the camera. - - -* Error messages are printed by the pylon Viewer when image acquisition is stopped. - When the pylon Viewer is started from a command line and image acquisition is - stopped, the Viewer prints out error messages such as - "Failed to grab image: GX status 0x81010017". This diagnostic output - indicates that image buffers have been cancelled by the pylon GigE vision - library on user request instead of being filled with image data. - This is expected behavior. - - -Version Infos -============= - -pylon has been built using the following tools. - -Linux x86 32bit/64bit: - Build system: Ubuntu 14.04 - Architecture: i686/x86_64 - libc-2.19.so - libstdc++.so.6.0.19 - libxcb.so.1.1.0 - libfontconfig.so.1.8.0 - libfreetype.so.6.11.1 - -Linux armhf: - Build system: Ubuntu 14.04 - Architecture: armv7l - libc-2.19.so - libstdc++.so.6.0.22 - libxcb.so.1.1.0 - libfontconfig.so.1.8.0 - libfreetype.so.6.11.1 - -Linux arm64: - Build system: Ubuntu 14.04 - Architecture: aarch64 (armv8) - libc-2.19.so - libstdc++.so.6.0.22 - libxcb.so.1.1.0 - libfontconfig.so.1.8.0 - libfreetype.so.6.11.1 From eb14481ba2a80842afde9b6f56ab26a8d1e944b4 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 2 Nov 2023 14:11:12 +0100 Subject: [PATCH 18/20] Fix comment --- bitbots_utils/src/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_utils/src/utils.cpp b/bitbots_utils/src/utils.cpp index c13dbeac..49b2506a 100644 --- a/bitbots_utils/src/utils.cpp +++ b/bitbots_utils/src/utils.cpp @@ -28,7 +28,7 @@ void wait_for_tf( // Store the beginning time auto start_time = clock->now(); - // Endless loop with increasing counter + // Endless loop while(rclcpp::ok()) { try { // Check if the frame we want to transform is known yet From 12774cf1ef9e6e57e65a26ae55498634963d3077 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 2 Nov 2023 14:11:59 +0100 Subject: [PATCH 19/20] Fix comment --- bitbots_utils/src/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_utils/src/utils.cpp b/bitbots_utils/src/utils.cpp index 49b2506a..479bdf6d 100644 --- a/bitbots_utils/src/utils.cpp +++ b/bitbots_utils/src/utils.cpp @@ -41,7 +41,7 @@ void wait_for_tf( continue; } - // Check if we can transform from base_link to all given frames + // Check if we can transform from the given root frame to all given frames if(!std::all_of( frames.begin(), frames.end(), From 66c768ed2c90e08bb58fafc5e8534a9d315b2b54 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 2 Nov 2023 14:13:10 +0100 Subject: [PATCH 20/20] Fix doc string --- bitbots_utils/include/bitbots_utils/utils.hpp | 1 + bitbots_utils/src/utils.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/bitbots_utils/include/bitbots_utils/utils.hpp b/bitbots_utils/include/bitbots_utils/utils.hpp index 6d95a148..5503c87d 100644 --- a/bitbots_utils/include/bitbots_utils/utils.hpp +++ b/bitbots_utils/include/bitbots_utils/utils.hpp @@ -19,6 +19,7 @@ namespace bitbots_utils * @param clock The clock to use for time * @param tf_buffer The tf buffer to use * @param frames The tf frames to wait for + * @param root_frame The root frame to transform from * @param check_interval Interval in which to check for the frames * @param warn_duration Duration after which to warn if the frames are not available * @param warn_interval Interval in which to keep warning if the frames are not available diff --git a/bitbots_utils/src/utils.cpp b/bitbots_utils/src/utils.cpp index 479bdf6d..f0c653a8 100644 --- a/bitbots_utils/src/utils.cpp +++ b/bitbots_utils/src/utils.cpp @@ -9,6 +9,7 @@ namespace bitbots_utils * @param clock The clock to use for time * @param tf_buffer The tf buffer to use * @param frames The tf frames to wait for + * @param root_frame The root frame to transform from * @param check_interval Interval in which to check for the frames * @param warn_duration Duration after which to warn if the frames are not available * @param warn_interval Interval in which to keep warning if the frames are not available