SDK测试

找到ugv_sdk功能包下面的demo进行测试
在这里插入图片描述
进入ugv_sdk文件夹，然后新建build文件夹并编译。每次更改上述scout_mini_omni_demo.cpp文件后，应该都需要编译一下。

cd ugv_sdk
mkdir build && cd build
cmake …
make
在这里插入图片描述

之后，我们就可以进入ugv_sdk/bulid文件夹下，运行对应的scout_mini_omni_demo。

cd ~/catkin_ws/src/ugv_sdk/bulid
./bin/scout_mini_omni_demo

编写Ros程序控制

Ros功能包分析

scout底盘驱动的功能包主要有scou_ros和ugv_sdk
在这里插入图片描述
scout_ros内部有三个小功能包：

scout_base: 底部控制层
scout_bringup: 应用层，主要是有launch文件
scout_msgs: scout的自定义消息

在这里插入图片描述

关键变量

其中，我们主要关注三个速度即可：

msg.twist.linear.x：X方向的速度 
msg.twist.angular.z：Z轴方向的转速 
msg.twist.linear.y：Y方向的速度

仅有linear.x可以控制底盘前后直线运动；仅有angular.z可以控制底盘原地顺时针或逆时针转动；仅有linear.y可以控制底盘左右平移运动；linear.x或linear.y配合angular.z可以实现底盘的转向；结合linear.x和linear.y可以实现底盘的斜向行进。我们通过发布对应的速度数据，就可以控制小车底盘的运动。小车的坐标系如下，箭头方向分别代表了对应坐标轴的正方向。
在这里插入图片描述

程序实现

我们需要在scout_base功能包的src目录下新建名为scout_control_demo1.cpp和speed.cpp的文件。
在这里插入图片描述

  • 速度控制
    增加speed.cpp
/*
  我们可以向 "/cmd_vel_raw" 发布控制指令
  
    Outline : 对发布的控制指令进行限幅
  1） 订阅话题 "/cmd_vel_raw"
    回调函数对话题信息中发布的速度进行限幅
    既：x方向的线速度 <= 0.3 || z方向的角速度 <= 0.5 
  2） 发布话题 "/cmd_vel"
    拷贝限幅后的话题消息，并发布

*/

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
geometry_msgs::Twist speed;
void callback(geometry_msgs::TwistStamped msg)
{
  // 速度限幅
  if(msg.twist.linear.x >= 0.3)
    msg.twist.linear.x = 0.3;
  if(msg.twist.angular.z >= 0.5)
    msg.twist.angular.z = 0.5;
  speed = msg.twist;
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "speed");
    ros::NodeHandle n;
    // 订阅话题 "/cmd_vel_raw" ，发布话题 "/cmd_vel"
    ros::Subscriber sub =n.subscribe("/cmd_vel_raw",50,callback);
    ros::Publisher pub =n.advertise<geometry_msgs::Twist>("/cmd_vel",5);
    ros::Rate r(50);
    while(ros::ok())
    {
      pub.publish(speed);
      ros::spinOnce();
      r.sleep();
    }

    return 0;
}
  • 控制程序scout_control_demo1.cpp
    将小车走矩形这个过程分为直行和旋转7个状态，通关改变状态变量在这7个状态中切换
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <scout_msgs/ScoutLightCmd.h>

#include <tf/transform_datatypes.h>

//速度控制话题消息类型类型
geometry_msgs::TwistStamped cmd_speed;
//小车x,y方向速度
double linear_x;
double linear_y;
​
//里程计发布的位置数据
double pose_x;
double pose_y;

// th
//转速
double angular_z;
//里程计信息
nav_msgs::Odometry odom_pose;
//当前旋转角度
double th;

void Odometry_Callback(const nav_msgs::Odometry::ConstPtr& msg)
{
    odom_pose = *msg;

    // 获取四元数信息
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);

    // 将四元数转换为欧拉角，并取出与 z 轴方向的旋转角度
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, th);
    th = th * (180.0 / M_PI);

    if (th < 0) {
        th += 360.0 ;
    }


}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scout_control_demo1");
    ros::NodeHandle n;

    // 发布话题 "/cmd_vel_raw"
    ros::Publisher pub = n.advertise<geometry_msgs::TwistStamped>("/cmd_vel_raw", 5);

    // 订阅里程计话题
    ros::Subscriber sub = n.subscribe<nav_msgs::Odometry>("/odom", 10, Odometry_Callback);
    //配合r.sleep控制循环频率
    ros::Rate r(50);

    // 设置底盘运动速度，初始前进
    linear_x = 0.1;
    angular_z = 0.0;
    linear_y = 0.0;
    // 运动状态标识符
    int tag = 0;

    while(ros::ok())
    {  
        cmd_speed.twist.linear.x = linear_x;
        cmd_speed.twist.linear.y = linear_y;
	    // th
	    cmd_speed.twist.angular.z = angular_z;

        pub.publish(cmd_speed);


        //获取里程计的位置信息
        pose_x = odom_pose.pose.pose.position.x;
        pose_y = odom_pose.pose.pose.position.y;

	    //输出当前的角度和运动状态
        ROS_INFO("current th & tag:%f %i" ,th, tag);


        //走矩形
	// 1 x方向直行到一定位置后，开始旋转，开始旋转状态1
        if (pose_x >= 0.2 && tag==0)	 
        {
            linear_x = 0;
            angular_z = 0.2;
            linear_y = 0;
            tag = 1;
	        ROS_INFO("1 pose:%f" ,th);
        } 

	// 2 旋转到90度，开始直行，切换状态2
        else if ( tag==1 && th >= 90)
        {
            linear_x = 0.1;
	        angular_z = 0;
            linear_y = 0;
            tag = 2;
            // ROS_INFO("Current pose:%f, %f", pose_x, pose_y);
	        ROS_INFO("2 pose:%f" ,th);
        }

	
	// 3 y方向直行到一定位置后开始旋转，切换状态3
	    else if (pose_y >= 0.2 && tag==2  )
        {
            linear_x = 0;
	        angular_z = 0.2;
            linear_y = 0;
            tag = 3;
            //ROS_INFO("Current pose:%f, %f", pose_x, pose_y);
            ROS_INFO("3 pose:%f" ,th);
        } 

	 
	// 4 旋转到180度，开始直行，切换状态4
        else if (tag==3 && th >= 180 )
        {
            linear_x = 0.1;
   	        angular_z = 0;
            linear_y = 0;
            tag = 4;
            //ROS_INFO("Current pose:%f, %f", pose_x, pose_y);
            ROS_INFO("4 pose:%f" ,th);
        } 

	// 5 x方向回到原点后，开始旋转，切换状态5
	    else if (tag==4 && pose_x <= 0 )
        {
            linear_x = 0;
   	        angular_z = 0.2;
            linear_y = 0;
            tag = 5;
            //ROS_INFO("Current pose:%f, %f", pose_x, pose_y);
            ROS_INFO("5 pose:%f" ,th);
        } 

	// 6 旋转270度后，开始直行，切换状态6
	    else if (tag==5 && th >= 270)
        {
            linear_x = 0.1;
   	        angular_z = 0;
            linear_y = 0;
            tag = 6;
            //ROS_INFO("Current pose:%f, %f", pose_x, pose_y);
            ROS_INFO("6 pose:%f" ,th);
        } 

        //y方向回到原点，停止运动
        else if (pose_y <= 0 && tag==6)
        {
            linear_x = 0;
            linear_y = 0;
	        angular_z = 0;
            
        }

        //当处理到ros::spinonce()时，会去话题订阅缓冲区中查看有没有回调函数，如果有则去处理回调函数，如果没有则继续往下执行
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

在功能包下的CMakeLists.txt文件中加入对应内容以包含该文件

# CMakeLists.txt文件中加入
## 自行编写的控制节点
add_executable(scout_control_demo1 src/scout_control_demo1.cpp)
target_link_libraries(scout_control_demo1 ${catkin_LIBRARIES})
add_executable(speed src/speed .cpp)
target_link_libraries(speed ${catkin_LIBRARIES})

运行例程

catkin_make编译之后，便可以运行对应的节点。

cd 到工作空间下
cd scout_ws/
catkin_make
·使能gs_usb内核模块 （不知道是不是每次都要）
sudo modprobe gs_usb

这个指令不一定每次都需要，有时候一上电他就自动连接了 （未插拔插口）
sudo ip link set can0 up type can bitrate 500000

1) 运行scout底盘节点对应的launch文件
source ./devel/setup.bash
roslaunch scout_bringup sout_robot_base.launch
​
2) 运行scout_mini_omni_base下的speed节点
#　他将会订阅我们发布的＇/cmd_vel_raw＇话题
source ./devel/setup.bash 
rosrun scout_base speed 
​
3） 运行我们编写的节点 
source ./devel/setup.bash
rosrun scout_base scout_control_demo1

我们这里也可以编写对应的launch文件，以便方便的运行这些节点。比如说，我们可以在scout_robot_base.launch文件中添加上scout_base功能包下的speed节点的启动命令，如下：

<launch>
    <arg name="port_name" value="can0" />
    <arg name="simulated_robot" value="false" />
    <arg name="odom_topic_name" default="odom" />
​
    <include file="$(find scout_mini_omni_base)/launch/scout_base.launch">
        <arg name="port_name" default="$(arg port_name)" />
        <arg name="simulated_robot" default="$(arg simulated_robot)" />
        <arg name="odom_topic_name" default="$(arg odom_topic_name)" />
    </include>
    
    <node name="speed" pkg="scout_base" type="speed" output="screen">
    </node>
</launch>

如此一来，我们就只需打开两个终端运行两条命令。这里并不建议将我们自行编写的scout_control_demo1节点添加到scout_robot_base.launch文件中，但是我们可以新建一个新的launch文件。如果我们把他添加进去了，那我们一运行launch文件，小车就会运动，这多少有些危险。

这个指令不一定每次都需要，有时候一上电他就自动连接了 （未插拔插口）
sudo ip link set can0 up type can bitrate 500000
​
cd 到工作空间下
cd scout_ws/
1) 运行scout底盘节点对应的launch文件
source ./devel/setup.bash
roslaunch scout_bringup sout_robot_base.launch
​
2） 运行我们编写的节点 
source ./devel/setup.bash
rosrun scout_base scout_control_demo1

在这里插入图片描述

【参考教程】
[1] https://zhuanlan.zhihu.com/p/550920775
[2] https://zhuanlan.zhihu.com/p/550922615
[3] VScode安装
https://www.python100.com/html/61W68HAA23QB.html
[4] 四元数转欧拉角
https://blog.csdn.net/qq_23670601/article/details/87968936?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522170020650016800186561119%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=170020650016800186561119&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allsobaiduend~default-2-87968936-null-null.142v96pc_search_result_base6&utm_term=%20%23include%20%3Ctf%2Ftransform_datatypes.h%3E&spm=1018.2226.3001.4187
[5] https://blog.csdn.net/YiYeZhiNian/article/details/130752843?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522170020596916800182176634%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=170020596916800182176634&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allfirst_rank_ecpm_v1~rank_v31_ecpm-5-130752843-null-null.142v96pc_search_result_base6&utm_term=ROS%E6%B1%82%E6%97%8B%E8%BD%AC%E8%A7%92%E5%BA%A6&spm=1018.2226.3001.4187
[6] ros::Rate 和ros::spin()，spinOnce()
https://blog.csdn.net/zeye5731/article/details/124314347?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522170020773816800226523604%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=170020773816800226523604&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allfirst_rank_ecpm_v1~rank_v31_ecpm-4-124314347-null-null.142v96pc_search_result_base6&utm_term=ros%3A%3ARate%20r%2850%29%3B&spm=1018.2226.3001.4187

# angular.js # 前端 # javascript
Logo
开放原子开发者工作坊

开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！

更多推荐

第二届开放原子大赛首批创新成果集结武汉，诚邀广大开发者共鉴开源技术盛宴

第二届开放原子大赛首批创新成果集结武汉，诚邀广大开发者共鉴开源技术盛宴

avatar 开放原子开发者工作坊

诚邀报名 | 开源基础设施能力建设分论坛：打造开源生态的“心脏”

诚邀报名 | 开源基础设施能力建设分论坛：打造开源生态的“心脏”

avatar 开放原子开发者工作坊

诚邀报名 | 编程语言分论坛：AI时代的技术革新与开源实践

诚邀报名 | 编程语言分论坛：AI时代的技术革新与开源实践

avatar 开放原子开发者工作坊