Class KickNode
Defined in File kick_node.hpp
Inheritance Relationships
Base Type
public rclcpp::Node
Class Documentation
-
class bitbots_dynamic_kick::KickNode : public rclcpp::Node
KickNode is that part of bitbots_dynamic_kick which takes care of interacting with ROS and utilizes a KickEngine to calculate actual kick behavior.
It provides an ActionServer for the bitbots_msgs::KickAction. This actionServer accepts new goals in any tf frame, and sets up the KickEngines to work towards this new goal
Additionally it publishes the KickEngines motor-goals back into ROS
Public Functions
-
explicit KickNode(const std::string &ns = std::string(), std::vector<rclcpp::Parameter> parameters = {})
Callback that gets executed whenever #server_ receives a new goal.
- Parameters
goal – New goal to process
-
bitbots_msgs::msg::JointCommand stepWrapper(double dt)
This wrapper is used in the python wrapper for a single step of the kick
- Parameters
dt – the time difference since the last call of this method
- Returns
the JointCommand representing the next step or an empty JointCommand if the kick is done
-
double getProgress()
Get the current progress of the kick, from 0 to 1
-
bool init(const bitbots_msgs::action::Kick::Goal &goal_msg, std::string &error_string)
Initialize the node
- Parameters
goal_msg – The goal_msg of the kick
error_string – when the return value is false, this will contain details about the error
trunk_to_base_footprint – transform from trunk to base_footprint
- Returns
whether the setup was successful
Set the current joint state of the robot
-
geometry_msgs::msg::Pose getTrunkPose()
Get the current pose of the trunk, relative to the support foot
-
bool isLeftKick()
Whether the left foot is the current kicking foot
Private Functions
Do main loop in which KickEngine::update() gets called repeatedly. The ActionServer’s state is taken into account meaning that a cancelled goal no longer gets processed.
-
bitbots_splines::JointGoals kickStep(double dt)
Execute one step of engine-stabilize-ik
- Returns
the motor goals
-
void publishSupportFoot(bool is_left_kick)
Publish the current support_foot so that a correct base_footprint can be calculated
- Parameters
is_left_kick – Whether the left foot is the current kicking foot, meaning it is in the air
-
bitbots_msgs::msg::JointCommand getJointCommand(const bitbots_splines::JointGoals &goals)
Get JointCommand message for JointGoals
-
rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector<rclcpp::Parameter> ¶meters)
Private Members
-
rclcpp::Publisher<bitbots_msgs::msg::JointCommand>::SharedPtr joint_goal_publisher_
-
rclcpp::Publisher<biped_interfaces::msg::Phase>::SharedPtr support_foot_publisher_
-
rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr cop_l_subscriber_
-
rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr cop_r_subscriber_
-
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_
-
rclcpp_action::Server<bitbots_msgs::action::Kick>::SharedPtr server_
-
KickEngine engine_
-
Stabilizer stabilizer_
-
Visualizer visualizer_
-
int engine_rate_
-
double last_ros_update_time_
-
std::unique_ptr<tf2_ros::Buffer> tf_buffer_
-
std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_
-
bool was_support_foot_published_
-
moveit::core::RobotStatePtr goal_state_
-
moveit::core::RobotStatePtr current_state_
-
OnSetParametersCallbackHandle::SharedPtr callback_handle_
-
bool currently_kicking_ = false
-
std::string base_link_frame_
-
std::string base_footprint_frame_
-
std::string l_sole_frame_
-
std::string r_sole_frame_
-
KickParams unstable_config_
-
KickParams normal_config_
-
explicit KickNode(const std::string &ns = std::string(), std::vector<rclcpp::Parameter> parameters = {})