ROS小车移动固定距离or转向固定角度

柔情只为你懂 2022-12-06 15:20 158阅读 0赞

\#include <ros/ros.h>  
\#include <signal.h>  
\#include <geometry\_msgs/Twist.h>  
\#include <string.h>

ros::Publisher cmdVelPub;

void shutdown(int sig)  
\{  
  cmdVelPub.publish(geometry\_msgs::Twist());//ctrl+c  
  ROS\_INFO("goforward cpp ended!");  
  ros::shutdown();  
\}

int main(int argc, char\*\* argv)  
\{  
  ros::init(argc, argv, "GoForward");//int ros  
  std::string topic = "/cmd\_vel";  
  ros::NodeHandle node;  
  cmdVelPub = node.advertise<geometry\_msgs::Twist>(topic, 1);  
//=========================  
  double rate = 10 ;  
  ros::Rate loopRate(rate);  
  float linear\_speed     = 0.2;  
  float goal\_distance     = 1.0;  
  float linear\_duration = goal\_distance / linear\_speed;

float angular\_speed    = 1.0;  
  float goal\_angle    = M\_PI;  
  float angular\_duration= goal\_angle / angular\_speed;

int count = 0;  
  int ticks;  
//================================  
  signal(SIGINT, shutdown);  
  ROS\_INFO("goforward cpp start...");  
  geometry\_msgs::Twist speed;   
//================================  
  while (ros::ok())  
  \{  
    speed.linear.x = linear\_speed;  
    ticks = int(linear\_duration\*rate);  
        for(int i = 0;i<ticks; i++)  
    \{  
    cmdVelPub.publish(speed);  
    loopRate.sleep();  
    \}  
     cmdVelPub.publish(geometry\_msgs::Twist());  
        ROS\_INFO("detect arm...!");  
    // stop&catch  
       speed.linear.x = 0;  
    ticks = int(linear\_duration\*rate);  
        for(int i = 0;i<ticks; i++)  
    \{  
    cmdVelPub.publish(speed);  
    loopRate.sleep();  
    \}  
    cmdVelPub.publish(geometry\_msgs::Twist());  
      ROS\_INFO("run again");  
    //run again  
    speed.linear.x = linear\_speed;  
    ticks = int(linear\_duration\*rate);  
        for(int i = 0;i<ticks; i++)  
    \{  
    cmdVelPub.publish(speed);  
    loopRate.sleep();  
    \}  
    speed.linear.x = 0;  
    cmdVelPub.publish(geometry\_msgs::Twist());  
    ROS\_INFO("stop");  
  \}

return 0;  
\}

转自：[https://blog.csdn.net/weixin\_42369496/article/details/104923906][https_blog.csdn.net_weixin_42369496_article_details_104923906]

[https_blog.csdn.net_weixin_42369496_article_details_104923906]: https://blog.csdn.net/weixin_42369496/article/details/104923906