Class RobotSimulator

Inheritance Relationships

Base Type

  • public rclcpp::Node

Class Documentation

class RobotSimulator : public rclcpp::Node

Tiny robot simulator: Takes cmd_vel msgs and updates a TF accordingly. Useful for simulating a robot’s movement in unit tests. The frames in the config_ member define the TF’s frame_id and child_frame_id.

Public Functions

RobotSimulator(const std::string &node_name = "robot_simulator", const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Initialises the simulator and starts with publishing an identity TF.

Public Members

std::string robot_frame_id = "base_link"
std::string parent_frame_id = "odom"
bool is_robot_stuck = false

Protected Functions

void velocityCallback(const geometry_msgs::msg::TwistStamped::SharedPtr vel)

Handle new command velocities. Incoming msgs need to be in the robot’s frame.

void setStateCallback(const std::shared_ptr<rmw_request_id_t> request_header, const mbf_msgs::srv::SetTestRobotState::Request::SharedPtr request, const mbf_msgs::srv::SetTestRobotState::Response::SharedPtr response)

Handle setting the robot state (pose, velocity) via service interface.

The pose jump induced by setting the robot’s state will not be reflected in the odometry msg stream published by this node. Setting the robot’s state behaves like kidnapping the robot, though the kidnapping time period is extremely small

void continuouslyUpdateRobotPose()

Regularly (via timer) updates the robot’s pose based on current_velocity and publishes it via tf2.

rcl_interfaces::msg::SetParametersResult setParametersCallback(std::vector<rclcpp::Parameter> parameters)

React to parameter changes.

Protected Attributes

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_parameters_callback_handle_
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_subscription_
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_
rclcpp::Service<mbf_msgs::srv::SetTestRobotState>::SharedPtr set_state_server_
geometry_msgs::msg::Twist current_velocity_
rclcpp::TimerBase::SharedPtr update_robot_pose_timer_
geometry_msgs::msg::TransformStamped trf_parent_robot_
struct mbf_test_utility::RobotSimulator::[anonymous] config_