L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

basic cmd_vel

Browse Source
This commit is contained in:
Wojciech Kwolek 2021-03-25 11:31:44 +00:00
parent 95f81b71c5
commit 50767ebc27
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/ros2aria/src/ros2aria.cpp
View File

@ -49,6 +49,16 @@ private:
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
{
RCLCPP_INFO(this->get_logger(), "cmd_vel: %f", msg->linear.x);
float x, y, z;
x = msg->linear.x;
y = msg->linear.y;
z = msg->angular.z;
this->robot->lock();
this->robot->setVel(x * 1e3);
if (robot->hasLatVel())
robot->setLatVel(y * 1e3);
robot->setRotVel(z * 180 / M_PI);
robot->unlock();
}
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;