Class DynupNode

Defined in File dynup_node.hpp

Class Documentation

class bitbots_dynup::DynupNode

DynupNode is that part of bitbots_dynamic_DynUp which takes care of interacting with ROS and utilizes a DynUpEngine to calculate actual DynUp behavior.

It provides an ActionServer for the bitbots_msgs::DynUpAction. This actionServer accepts new goals in any tf frame, and sets up the DynUpEngines to work towards this new goal

Additionally it publishes the DynUpEngines motor-goals back into ROS

Public Functions

explicit DynupNode(rclcpp::Node::SharedPtr node, const std::string &ns = "", std::vector<rclcpp::Parameter> parameters = {})

void onSetParameters()

void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)

void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr joint_states)

DynupEngine *getEngine()

DynupIK *getIK()

bitbots_dynup::msg::DynupPoses getCurrentPoses()

Retrieve current positions of left foot and trunk relative to right foot

Returns: The pair of (right foot, left foot) poses if transformation was successful

bitbots_msgs::msg::JointCommand step(double dt)

bitbots_msgs::msg::JointCommand step(double dt, const sensor_msgs::msg::Imu::SharedPtr imu_msg, const sensor_msgs::msg::JointState::SharedPtr joint_state_msg)

geometry_msgs::msg::PoseArray step_open_loop(double dt)

void reset(int time = 0)

Private Functions

rclcpp_action::GoalResponse goalCb(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const DynupGoal::Goal> goal)

Callback that gets executed whenever #m_server receives a new goal.

Parameters: goal – New goal to process

rclcpp_action::CancelResponse cancelCb(std::shared_ptr<DynupGoalHandle> goal)

void acceptedCb(const std::shared_ptr<DynupGoalHandle> goal)

void execute(const std::shared_ptr<DynupGoalHandle> goal)

void loopEngine(int, std::shared_ptr<DynupGoalHandle> goal_handle): Do main loop in which DynUpEngine::tick() gets called repeatedly. The ActionServer’s state is taken into account meaning that a cancelled goal no longer gets processed.

void publishSupportFoot(bool is_left_dyn_up)

Publish the current support_foot so that a correct base_footprint can be calculated

Parameters: is_left_dyn_up – Whether the left foot is the current DynUping foot, meaning it is in the air

bitbots_msgs::msg::JointCommand createGoalMsg(const bitbots_splines::JointGoals &goals): Creates the Goal Msg

double getTimeDelta(): Helper method to achieve correctly sampled rate

Private Members

rclcpp::Node::SharedPtr node_

rclcpp_action::Server<DynupGoal>::SharedPtr action_server_

std::vector<std::string> param_names_

bitbots_dynup::ParamListener param_listener_

bitbots_dynup::Params params_

DynupEngine engine_

Stabilizer stabilizer_

Visualizer visualizer_

DynupIK ik_

int stable_duration_ = 0

int failed_tick_counter_ = 0

double last_ros_update_time_ = 0

double start_time_ = 0

bool server_free_ = true

bool debug_ = false

bool cancel_goal_ = false

tf2_ros::Buffer tf_buffer_

tf2_ros::TransformListener tf_listener_

std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_

moveit::core::RobotModelPtr kinematic_model_

rclcpp::Publisher<bitbots_msgs::msg::JointCommand>::SharedPtr joint_goal_publisher_

rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_

rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_