-
Notifications
You must be signed in to change notification settings - Fork 1k
/
MyRobotDriver.cpp
53 lines (42 loc) · 1.63 KB
/
MyRobotDriver.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#include "my_package/MyRobotDriver.hpp"
#include "rclcpp/rclcpp.hpp"
#include <cstdio>
#include <functional>
#include <webots/motor.h>
#include <webots/robot.h>
#define HALF_DISTANCE_BETWEEN_WHEELS 0.045
#define WHEEL_RADIUS 0.025
namespace my_robot_driver {
void MyRobotDriver::init(
webots_ros2_driver::WebotsNode *node,
std::unordered_map<std::string, std::string> ¶meters) {
right_motor = wb_robot_get_device("right wheel motor");
left_motor = wb_robot_get_device("left wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_velocity(left_motor, 0.0);
wb_motor_set_position(right_motor, INFINITY);
wb_motor_set_velocity(right_motor, 0.0);
cmd_vel_subscription_ = node->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", rclcpp::SensorDataQoS().reliable(),
[this](const geometry_msgs::msg::Twist::SharedPtr msg){
this->cmd_vel_msg.linear = msg->linear;
this->cmd_vel_msg.angular = msg->angular;
}
);
}
void MyRobotDriver::step() {
auto forward_speed = cmd_vel_msg.linear.x;
auto angular_speed = cmd_vel_msg.angular.z;
auto command_motor_left =
(forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) /
WHEEL_RADIUS;
auto command_motor_right =
(forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) /
WHEEL_RADIUS;
wb_motor_set_velocity(left_motor, command_motor_left);
wb_motor_set_velocity(right_motor, command_motor_right);
}
} // namespace my_robot_driver
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(my_robot_driver::MyRobotDriver,
webots_ros2_driver::PluginInterface)