From 3d86c9548c5eda769ef72a65c4c520ac3df8b147 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 13 Jul 2021 18:36:36 +0800 Subject: [PATCH] fixed motion command --- demo/scout_demo/scout_robot_demo.cpp | 4 ++-- .../details/robot_base/agilex_base.hpp | 22 ++++++++++++++----- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/demo/scout_demo/scout_robot_demo.cpp b/demo/scout_demo/scout_robot_demo.cpp index 250a162..ac7ea2c 100644 --- a/demo/scout_demo/scout_robot_demo.cpp +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -76,8 +76,8 @@ int main(int argc, char **argv) { while (true) { // motion control // if (count < 100) { - // std::cout << "Motor: 0.2, 0" << std::endl; - // scout->SetMotionCommand(0.2, 0.0); + std::cout << "Motor: 0.2, 0" << std::endl; + scout->SetMotionCommand(0.2, 0.0); // } // get robot state diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp index ae04b4a..fdccbfd 100644 --- a/include/ugv_sdk/details/robot_base/agilex_base.hpp +++ b/include/ugv_sdk/details/robot_base/agilex_base.hpp @@ -55,12 +55,24 @@ class AgilexBase : public RobotInterface { if (can_connected_) { // motion control message AgxMessage msg; - msg.type = AgxMsgMotionCommand; - msg.body.motion_command_msg.linear_velocity = linear_vel; - msg.body.motion_command_msg.angular_velocity = angular_vel; - msg.body.motion_command_msg.lateral_velocity = lateral_velocity; - msg.body.motion_command_msg.steering_angle = steering_angle; + if (parser_.GetProtocolVersion() == ProtocolVersion::AGX_V1) { + msg.type = AgxMsgMotionCommandV1; + msg.body.v1_motion_command_msg.control_mode = CONTROL_MODE_CAN; + msg.body.v1_motion_command_msg.clear_all_error = false; + msg.body.v1_motion_command_msg.linear = linear_vel; + msg.body.v1_motion_command_msg.angular = angular_vel; + msg.body.v1_motion_command_msg.lateral = lateral_velocity; + } else if (parser_.GetProtocolVersion() == ProtocolVersion::AGX_V2) { + msg.type = AgxMsgMotionCommand; + msg.body.motion_command_msg.linear_velocity = linear_vel; + msg.body.motion_command_msg.angular_velocity = angular_vel; + msg.body.motion_command_msg.lateral_velocity = lateral_velocity; + msg.body.motion_command_msg.steering_angle = steering_angle; + } + std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel + << std::endl; + // send to can bus can_frame frame; if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);