Ros学习日记2-2021/7/24

这几天没遇到什么很困难的问题，前几天发现网速很慢，网上说是dns的问题，折腾了好久，最后把网络给搞崩了，最后只能重装了一下系统，后来发现好像是火狐浏览器的问题，用这个浏览器上网就巨慢，b站都卡的不行，换了chrome之后居然就好了....

之后这几天发现写service client的时候经常会忘记加上这个函数等待。

client.waitForExistence();

所以经常报错，这个以后要多注意一下。

现在遇到的一个比较困难的问题是在做乌龟的跟踪实验时出现的。

我根据奥特学园的教程走，在203P的tf乌龟跟踪实验中，需要用roslaunch配合main函数的arg

和argv两个参数来实现对两只乌龟tf坐标变换的获取，根据教程，我的launch文件写成了这样：

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

    <node pkg="turtle_follow" type="new_turtle" name="turtle2" output="screen" />

    <node pkg="turtle_follow" type="turtle_pose_pub" name="turtle_pose_pub1" output="screen" args="turtle1"/>
    <node pkg="turtle_follow" type="turtle_pose_pub" name="turtle_pose_pub2" output="screen" args="turtle2"/>
</launch>

和教程一样，但是却无法启动，无法发布乌龟的数据，在反复检查了程序后，我用rosrun的指令运行了一下，发现没有问题，于是我就知道是launch文件出现了问题。

具体问题

因为在教程中，会根据argc来进行判断argv的参数数量，并进行报错处理，所以我直接就定位了这个问题，使用了一个for循环输出了argv捕获到的所有参数：

    if(argc !=2 )
    {
        ROS_ERROR("参数输入错误，应输入一个参数，输入了%d个参数",argc);
        for(int i = 0; i < argc; i++)
        {
            ROS_ERROR("第%d个参数是%s",argc,argv[i]);
        }

        return 0;
    }

 可以发现，第一个参数是文件名，这个没有问题，第二个参数是我用launch文件传进去的，也没有问题，但是第三个和第四个参数出现问题了，这两个参数我并没有传入，但是却被argv给捕获到了，导致程序出现异常。

问题原因

这个问题我在百度上找了很久都没有找到原因，最后在google上找到了：问题原因

里面的回答是这样的：

大概就是说：当rosrun运行的时候，只传入特殊声明的参数（也就是后面输入的参数），但是roslaunch总是会加入__name等参数去设置节点名等，话题名的映射也是用这种命令行参数的方式操作的，然后就是要确定节点可以接收ros的特殊参数或者干脆就直接避免命令行参数，大多数情况下用参数服务器是更好的选择。

针对这个问题，我暂时选择了用两个cpp文件的方式将教程的发布乌龟tf坐标变换数据功能实现了，当然也可以把判断中argc改成4，然后读取argv[1]的数据，此外则是可以改用参数服务器来进行操作。

如何使用参数服务器来进行操作，还要后面有时间再看一下。

方案：使用参数服务器中的参数来读入乌龟的名字。

在读取乌龟名字的时候，因为/turtle1/pose和/turtle2/pose这个话题名已经固定了，需要读取的乌龟名字就是turtle1和turtle2这两个string，如果这个参数是一个私有变量，那么在读取的时候就要读取/节点名/参数名，而节点名字是在launch文件中写入的，在程序中就需要通过__name这个参数来读取，很麻烦，所以我将这个参数设置为了全局变量，保证这两个节点不需要修改程序便能直接复用读取。

使用参数服务器实现程序复用时候遇到了一个问题：由于launch文件不能指定节点的启动顺序，而且在调用第二个节点时需要改变这个string变量，这就需要在第一个节点中将string变量进行修改，来让第二个节点成功调用。

为此，我在launch文件中设置了两个参数，一个是乌龟名字turtlename，用来以后代码复用的时候方便进行修改，另一个是乌龟的编号turtlecnt，用来记录启动次数，以此来改变turtle1和turtle2后面这个1/2：

<launch>
    <param name="turtlename" type="string" value="turtle" />
    <param name="turtlecnt" type="int" value="1" />

    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
    <node pkg="turtle_follow" type="new_turtle" name="turtle2" output="screen" />
    <node pkg="turtle_follow" type="turtle_pose_pub" name="turtle_pose_pub1" output="screen" />
    <node pkg="turtle_follow" type="turtle_pose_pub" name="turtle_pose_pub2" output="screen" />
</launch>

之后，在节点中对这两个参数进行读取、组合和操作：

    if(flag != true)
    {
        ROS_ERROR("乌龟名读取错误！");
        return 1;
    }
    else
        ROS_INFO("乌龟名读取参数：%s",turtlename.c_str());
    

    flag = n.getParam("turtlecnt",turtlecnt);

    if(flag != true)
    {
        ROS_ERROR("乌龟编号读取错误！");
        return 1;
    }
    else
        ROS_INFO("乌龟编号读取参数：%d",turtlecnt);

    ros::Subscriber sub = n.subscribe(turtlename + std::to_string(turtlecnt) + "/pose",10,func);

    turtlecnt++;

    n.setParam("turtlecnt",turtlecnt);

最终完整程序如下所示：

#include"ros/ros.h"
#include"turtlesim/Pose.h"
#include"tf2_ros/transform_broadcaster.h"
#include"geometry_msgs/TransformStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"tf2/LinearMath/Quaternion.h"

std::string turtlename;
int turtlecnt;

void func(const turtlesim::Pose::ConstPtr &p)
{
    static tf2_ros::TransformBroadcaster pub;
    static geometry_msgs::TransformStamped tfs;

    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtlename+ std::to_string(turtlecnt-1);
    tfs.transform.translation.x = p->x;
    tfs.transform.translation.y = p->y;
    tfs.transform.translation.z = 0.0;

    tf2::Quaternion qtn;

    qtn.setRPY(0,0,p->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    pub.sendTransform(tfs);    
}

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

    setlocale(LC_ALL,"");

    ros::init(argc,argv,"turtle_pose_pub");
    
    ros::NodeHandle n;

    bool flag = n.getParam("turtlename",turtlename);

    if(flag != true)
    {
        ROS_ERROR("乌龟名读取错误！");
        return 1;
    }
    else
        ROS_INFO("乌龟名读取参数：%s",turtlename.c_str());
    

    flag = n.getParam("turtlecnt",turtlecnt);

    if(flag != true)
    {
        ROS_ERROR("乌龟编号读取错误！");
        return 1;
    }
    else
        ROS_INFO("乌龟编号读取参数：%d",turtlecnt);

    ros::Subscriber sub = n.subscribe(turtlename + std::to_string(turtlecnt) + "/pose",10,func);

    turtlecnt++;

    n.setParam("turtlecnt",turtlecnt);

    ros::spin();

    return 0;
}

运行roslaunch进行测试：

程序成功运行，两个乌龟的坐标已经被两个节点分别订阅了，用rviz看一下：

两只乌龟的tf动态坐标变换都已经发布成功。