ros学习2~对应1-15讲练习

最新推荐文章于 2024-03-27 22:37:16 发布

dna葡萄糖

最新推荐文章于 2024-03-27 22:37:16 发布

阅读量294

点赞数

分类专栏： ros学习

本文链接：https://blog.csdn.net/qq_35942419/article/details/116143011

版权

ros学习专栏收录该内容

9 篇文章 1 订阅

订阅专栏

题目来源：https://blog.csdn.net/weixin_40689871/article/details/108186903

http://wiki.ros.org/ROS/Tutorials

1、2、3、对应第一个链接的深蓝课程作业

4、设计一个加法器，从命令行输入两个参数，返回两个参数的和。

打算采用ide-KDevelop进行debug，结果使用过程中出现了恶劣bug（重启后提示roscore未安装，rosrun找不到装的库），错误可复现，初步怀疑是ros环境问题，嫌弃麻烦，遂放弃。

第一题：和深蓝视频教学示例差不多两个凑一起就好了，无难度。


#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include"turtlesim/Pose.h"

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"homework1");
    ros::NodeHandle n;
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    ros::Subscriber pos_sub = n.subscribe("/turtle1/pose",10,poseCallback);
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;
        
        turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
vel_msg.linear.x, vel_msg.angular.z);
        ros::spinOnce();
        loop_rate.sleep();
        
    }
    
    return 0;
    
}

第二题：同样无难度


#include<ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc,char **argv)
{
    ros::init(argc,argv,"homework2");
    ros::NodeHandle n;
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    srv.request.x = 5.0;
    srv.request.y = 5.0;
    srv.request.name = "turtle2";
    
    ROS_INFO("Call service to spwan turtle[x:%0.6f,y:%0.6f,name:%s]",srv.request.x,srv.request.y,srv.request.name.c_str());
    add_turtle.call(srv);
    ROS_INFO("Spwan turtle successfully [name:%s]",srv.response.name.c_str());
    return 0;
    
}

第三题：

分成两个节点完成这个任务
第一个节点功能是生成一个新的海龟，生成海龟的功能已经由spawn.h实现了，这个功能以服务的方式存在，因此只需要采用客户端，对这个服务进行req就好了。

/opt/ros/melodic/share/turtlesim/srv/Spawn.srv文件内容如下

float32 x
float32 y
float32 theta
string name # Optional. A unique name will be created and returned if this is empty
---
string name

从题目中可知，需要命令行输入name，然后位置不重叠。因此req需要输入x，y，name
想法是：建立客户端，乌龟名字由命令行指定，在获取指令以后，先查询所有乌龟的位置（先查找有多少个乌龟，然后依次订阅所有乌龟pose话题，获得位置）（或者假定所有的乌龟都是这个客户端生成的，建立全局变量记录调用次数），然后生成一个不重叠坐标，随后想到这个方法太麻烦了，遂放弃。
随后想法是：建立客户端，乌龟名字由命令行指定，在获取指令以后，进行一个随机数获取，生成srv-req内容，然后向spawn服务器申请。


#include<ros/ros.h>
#include<turtlesim/Spawn.h>
#include"time.h"


int main(int argc,char **argv)
{
    if(argc!=2)
    {
        ROS_INFO("usage:please input turtle`s name:");
        return 1;
    }
    ros::init(argc,argv,"homework3");
    ros::NodeHandle n;
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    srand(time(NULL));
    srv.request.x = random()/100%10;
    srv.request.y = random()/100%10;
    srv.request.name = argv[1];
    
    ROS_INFO("Call service to spwan turtle[x:%0.6f,y:%0.6f,name:%s]",srv.request.x,srv.request.y,srv.request.name.c_str());
    add_turtle.call(srv);
    ROS_INFO("Spwan turtle successfully [name:%s]",srv.response.name.c_str());
    return 0;
}

第二个节点功能是控制运动

任务目标是控制任意海龟停止/启动，并给予速度。

停止等价于速度=0，因此本质还是控制海龟速度。

通过前面的学习知道，在创建对一个对象以后，会自动生成一个订阅端，订阅/<海龟名>/cmd_vel消息，如果要控制海龟移动，我们只需要给这个端发布消息即可。

/opt/ros/melodic/share/geometry_msgs/msg/Twist.msg文件内容如下

# This expresses velocity in free space broken into its linear and angular parts.
Vector3 linear
Vector3 angular
从任务需求上看，输入值有3个：海龟名字，停止启动标志，速度/角速度

建立发布端，从命令行获得命令，然后发布命令

对于如何输入控制参数，python实现很简单，利用argparse就可以实现命令行可选参数，c++中命令行可选输入实在是太难了，所以直接读参数文件。后续可以利用roslaunch读取参数服务器。

采用jsoncpp对参数文件进行读取，然后发布出去即可（停止的话，就直接把默认参数（都是0）发出去就好了）。


#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<fstream>
#include<jsoncpp/json/json.h>
//输入参数 海龟名 启动停止位 参数文件目录
//json文件示例。
/*
{
    "linear":
    {
        "x":1.0,
        "y":2.0,
        "z":0.0
    },
    "angular":
    {
        "x":0.0,
        "y":0.0,
        "z":5.0
    }
}
*/
geometry_msgs::Twist LoadParameters(std::string file_name)
{

    std::ifstream file;
    file.open(file_name,std::ios::binary);
    
    if(!file.is_open())
    {
        std::cout<<"open json file failed"<<std::endl;
    }
    
    Json::Reader reader;
    Json::Value root;
    
    geometry_msgs::Twist vel_msg;
    if(reader.parse(file,root))
    {
        vel_msg.linear.x = root["linear"]["x"].asDouble();
        vel_msg.linear.y = root["linear"]["y"].asDouble();
        vel_msg.linear.z = root["linear"]["y"].asDouble();
        vel_msg.angular.x = root["angular"]["x"].asDouble();
        vel_msg.angular.y = root["angular"]["y"].asDouble();
        vel_msg.angular.z = root["angular"]["z"].asDouble();        
//         std::cout << "Reading Complete!" << std::endl;

    }
    
    file.close();
    return vel_msg;
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"homework3_2");
    ros::NodeHandle n;
    if(argc <= 3 && argv[2] != 0)
    {
        ROS_INFO("usage:please input turtle control command");
        return -1;
        
    }
    std::string name = argv[1];
    name.insert(0,"/");
    name.append("/cmd_vel");
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>(name,10);
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        geometry_msgs::Twist vel_msg;
        if (argv[2] != 0)
        {
            vel_msg = LoadParameters(argv[3]);
        }
        turtle_vel_pub.publish(vel_msg);
        loop_rate.sleep();
        
    }
    return 0;   
}

由于客户端使用了jsoncpp的库，因此编译的时候加句

link_libraries(/usr/lib/x86_64-linux-gnu/)

target_link_libraries(homework3_2 ${catkin_LIBRARIES} jsoncpp)
4、要实现的功能是“求和”，由于目前没有节点能满足这个功能，因此我们需要编写一个服务器来实现这个功能。同时应用客户端来进行参数的输入和传递。

首先创建一个srv，其req是两个参数，res是求和后的结果

Add.srv

float32 a
float32 b
---
float32 result

编译这个服务；

创建服务器：

#include<ros/ros.h>
#include"learning_test/Add.h"

bool AddCallback(learning_test::Add::Request &req,learning_test::Add::Response &res)
{
    ROS_INFO("Add:%f + %f = ", req.a, req.b);
    
	res.result = req.a + req.b;
    
    return true;
}

int main(int argc,char** argv)
{
    ros::init(argc,argv,"add_server");
    
    ros::NodeHandle n;
    
    ros::ServiceServer Add_service = n.advertiseService("/add_calculate",AddCallback);
    
    ROS_INFO("Ready to input 2 nums!");
    
    ros::spin();
    
    return 0;
    
}

创建客户端：



#include<ros/ros.h>
#include"learning_test/Add.h"

int main(int argc,char** argv)
{
    if (argc < 3)
    {
        ROS_INFO("please input 2 nums");
        return -1;
    }
    ros::init(argc,argv,"add_client");
    ros::NodeHandle n;
    ros::service::waitForService("/add_calculate");
    ros::ServiceClient add_client = n.serviceClient<learning_test::Add>("/add_calculate");
    
    learning_test::Add srv;
    srv.request.a = atof(argv[1]);
    srv.request.b = atof(argv[2]);
    
    add_client.call(srv);
    ROS_INFO("Show calculate result:%f",srv.response.result);
    return 0;
}