diff --git a/README.md b/README.md index 56a4dba..ade5936 100644 --- a/README.md +++ b/README.md @@ -77,7 +77,96 @@ sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard roslaunch ranger_bringup ranger_teleop_keyboard.launch ``` -## Examples + + + + + + +---- + +## Ros Topic Examples + +* Input: the car linear velocity and the heading angle +* Output: total linear velocity, x direction velocity, y direction velocity, angular velocity, central steer angle and rotate radius, .etc + +### Publish topic to control the car + +see `ranger_ros/ranger_examples/src/input.cpp` for details + +```c++ +////----------------control by ros topic--------------------------------- +ros::Publisher motion_mode = + node.advertise("/ranger_setting", 1); +ranger_msgs::RangerSetting setting; +setting.motion_mode = ranger_msgs::RangerSetting::MOTION_MODE_ACKERMAN; +motion_mode.publish(setting); + +////------------------move by ros topic -------------------------------- +ros::Publisher move_cmd = + node.advertise("/cmd_vel", 10); +geometry_msgs::Twist cmd; +cmd.linear.x = 0.1; // the motor will run at 0.1m/s +cmd.angular.z = 30.0 / 180.0 * M_PI; // the heading angle of the car + +// publish robot state at 50Hz while listening to twist commands +ros::Rate rate(50); +while (ros::ok()) { + ros::spinOnce(); + + // /cmd_vel topic must send at 50Hz, even stop need send 0m/s + move_cmd.publish(cmd); + + rate.sleep(); +} +``` + +or publish by command line + +```shell +rostopic pub -1 /cmd_vel geometry_msgs/Twist -- '[0.1, 0.0, 0.0]' '[0.0, 0.0, 0.52358]' # 0.52358 = 30 degree +``` + + + +### Subcribe the car output + +see `ranger_ros/ranger_examples/src/output.cpp` for details + +```c++ +ros::Subscriber status_sub = node.subscribe( + "/ranger_status", 10, StatusCallback); +``` + +```c++ +void StatusCallback(ranger_msgs::RangerStatus::ConstPtr msg) { + std::cout << "linear velocity: " << msg->linear_velocity << std::endl; + std::cout << "angular velocity: " << msg->angular_velocity << std::endl; + std::cout << "x direction linear velocity: " << msg->x_linear_vel << std::endl; + std::cout << "y direction linear linear velocity: " << msg->y_linear_vel << std::endl; + std::cout << "rotate radius: " << msg->motion_radius << std::endl; + std::cout << "car heading angle: " << msg->steering_angle << std::endl; + // ...etc +} +``` + + + +or show the data by rostopic + +```shell +rostopic echo /ranger_status +``` + + + + + + + +---- + +## Sdk API Examples ### 0. enable can control diff --git a/ranger_examples/CMakeLists.txt b/ranger_examples/CMakeLists.txt index dfb1067..009ac49 100644 --- a/ranger_examples/CMakeLists.txt +++ b/ranger_examples/CMakeLists.txt @@ -143,6 +143,10 @@ add_executable(change_the_mode src/change_the_mode.cpp) target_link_libraries(change_the_mode ${catkin_LIBRARIES}) add_executable(control_the_car src/control_the_car.cpp) target_link_libraries(control_the_car ${catkin_LIBRARIES}) +add_executable(input src/input.cpp) +target_link_libraries(input ${catkin_LIBRARIES}) +add_executable(output src/output.cpp) +target_link_libraries(output ${catkin_LIBRARIES}) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/ranger_examples/src/control_the_car.cpp b/ranger_examples/src/control_the_car.cpp index 547a6fc..4052eae 100644 --- a/ranger_examples/src/control_the_car.cpp +++ b/ranger_examples/src/control_the_car.cpp @@ -47,6 +47,7 @@ int main(int argc, char *argv[]) { // publish robot state at 50Hz while listening to twist commands ros::Rate rate(50); while (ros::ok()) { + // send command in 50hz, nor will timeout robot->SetMotionCommand(l_v, angle_z); ros::spinOnce(); rate.sleep(); diff --git a/ranger_examples/src/input.cpp b/ranger_examples/src/input.cpp new file mode 100644 index 0000000..571ed60 --- /dev/null +++ b/ranger_examples/src/input.cpp @@ -0,0 +1,69 @@ +/** + * @Kit : Qt-Creator: Desktop + * @Author : Wang Zhe + * @Date : 2021-04-26 18:47:25 + * @FileName : input.cpp + * @Mail : zhe.wang@agilex.ai + * Copyright : AgileX Robotics (2021) + **/ + +#include + +#include +#include + +#include +#include +#include "ranger_base/ranger_messenger.hpp" +#include "ugv_sdk/ranger_base.hpp" + +using namespace westonrobot; + +std::shared_ptr robot; + +int main(int argc, char *argv[]) { + // setup ROS node + ros::init(argc, argv, "input"); + ros::NodeHandle node(""), private_node("~"); + + // instantiate a robot object + robot = std::make_shared(); + + robot->Connect("can0"); + robot->EnableCommandedMode(); + + ////-----------------direct control by call the sdk function------------- + // 0 Arckmann 1 Slide 2 round, 3 Sloping + robot->SetMotionMode(0); + // robot->SetMotionMode(1); + // robot->SetMotionMode(2); + // robot->SetMotionMode(3); + + // robot->SetMotionCommand(0.1, 30.0/180.0 * M_PI); // steer angle = 30° + + ////----------------control by ros topic--------------------------------- + ros::Publisher motion_mode = + node.advertise("/ranger_setting", 1); + ranger_msgs::RangerSetting setting; + setting.motion_mode = ranger_msgs::RangerSetting::MOTION_MODE_ACKERMAN; + motion_mode.publish(setting); + + ////------------------move by ros topic -------------------------------- + ros::Publisher move_cmd = + node.advertise("/cmd_vel", 10); + geometry_msgs::Twist cmd; + cmd.linear.x = 0.1; // the motor will run at 0.1m/s + cmd.angular.z = 30.0 / 180.0 * M_PI; // the heading angle of the car + + // publish robot state at 50Hz while listening to twist commands + ros::Rate rate(50); + while (ros::ok()) { + ros::spinOnce(); + + // /cmd_vel topic must send at 50Hz, even stop need send 0m/s + move_cmd.publish(cmd); + + rate.sleep(); + } + return 0; +} diff --git a/ranger_examples/src/output.cpp b/ranger_examples/src/output.cpp new file mode 100644 index 0000000..e3ec0a0 --- /dev/null +++ b/ranger_examples/src/output.cpp @@ -0,0 +1,40 @@ +/** + * @Kit : Qt-Creator: Desktop + * @Author : Wang Zhe + * @Date : 2021-04-26 18:47:49 + * @FileName : output.cpp + * @Mail : zhe.wang@agilex.ai + * Copyright : AgileX Robotics (2021) + **/ + +#include +#include +#include + +void StatusCallback(ranger_msgs::RangerStatus::ConstPtr msg) { + std::cout << "linear velocity: " << msg->linear_velocity << std::endl; + std::cout << "angular velocity: " << msg->angular_velocity << std::endl; + std::cout << "x direction linear velocity: " << msg->x_linear_vel + << std::endl; + std::cout << "y direction linear linear velocity: " << msg->y_linear_vel + << std::endl; + std::cout << "rotate radius: " << msg->motion_radius << std::endl; + std::cout << "car heading angle: " << msg->steering_angle << std::endl; + // ...etc +} + +int main(int argc, char *argv[]) { + // setup ROS node + ros::init(argc, argv, "output"); + ros::NodeHandle node(""), private_node("~"); + + ros::Subscriber status_sub = node.subscribe( + "/ranger_status", 10, StatusCallback); + + ros::Rate rate(50); + while (ros::ok()) { + ros::spinOnce(); + rate.sleep(); + } + return 0; +}