ROS机器人编程与SLAM算法解析第二章

14 篇文章 1 订阅

 节点

PubForBeginner.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.26

    Description: Simple ros publish demo for beginning learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_node PubForBeginner
    ****************
*/
#include "ros/ros.h"
#include "std_msgs/String.h"

int main(int argc,char** argv)
{
    //initialize and name node
    ros::init(argc,argv,"publisher");
    //create nodehandle
    ros::NodeHandle nh;
    //create publisher
    ros::Publisher simplepub = nh.advertise<std_msgs::String>("string_topic",100);
    // publish frequency
    ros::Rate rate(10);
    //message for publish
    std_msgs::String pubinfo;
    pubinfo.data="Hello, I'm Publisher!";
    while(ros::ok())
    {   
        simplepub.publish(pubinfo);
        rate.sleep();
    }
    return 0;
}

SubForBeginner.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.26

    Description: simple ros subscriber demo for beginning learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_node SubForBeginner
    ****************
*/ 

#include "ros/ros.h"
#include "std_msgs/String.h"

using namespace std;

void subCallback(const std_msgs::String& submsg)
{
    string subinfo;
    subinfo = submsg.data;
    ROS_INFO("The message subscribed is: %s",subinfo.c_str());
}
int main(int argc,char** argv)
{
    //initial and name node
    ros::init(argc,argv,"subscriber");
    //create nodehandle
    ros::NodeHandle nh;
    //create subscriber
    ros::Subscriber sub = nh.subscribe("string_topic",1000,&subCallback);
    ros::spin();
    return 0;
}

消息

Student.msg

string name
float64 height
float64 weight

Book_MyMsgPub.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.27

    Description: Simple ros custom define message publisher demo for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_message Book_MyMsgPub LiLei 180 160 
    ****************
*/

#include "ros/ros.h"
#include "book_message/Student.h"
#include <cstdlib>

using namespace std;
int main(int argc,char** argv)
{
    //initial and name node
    ros::init(argc,argv,"node_MyMsgPub");
    if(argc!=4)
    {
        cout<<"Error command parameter!\n"\
        <<"Please run command eg:\n"\
        <<"rosrun book_message Book_MyMsgPub LiLei 180 160"<<endl;
        return 1;
    }

    //create node handle
    ros::NodeHandle nh;

    //create message publisher
    ros::Publisher MyMsgPub =  nh.advertise<book_message::Student>("MyMsg",100);
    book_message::Student  sdtMsg;
    sdtMsg.name = argv[1];
    //convert string in argv to float and pass value to height, weight
    sdtMsg.height = atof(argv[2]);
    sdtMsg.weight = atof(argv[3]);

    ros::Rate rate(10);
    while(ros::ok())
    {
        MyMsgPub.publish(sdtMsg);
        rate.sleep();
    }

    return 0;
}

Book_MyMsgSub.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.27

    Description: Simple ros custom define message subscriber demo for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_message Book_MyMsgSub
    ****************
*/

#include "ros/ros.h"
#include "book_message/Student.h"

// custom defined message callback function
void MyMsgCallback(const book_message::Student& sdtInfo)
{
    ROS_INFO("The student information is:\n name:%s---height:%f---weight:%f",
                sdtInfo.name.c_str(),
                sdtInfo.height,
                sdtInfo.weight);
}

int main(int argc, char** argv)
{
    //initial and name node
    ros::init(argc,argv,"node_MyMsgSub");
    //create node handle
    ros::NodeHandle nh;
    //create subscriber
    ros::Subscriber MyMsgSub = nh.subscribe("MyMsg",1000,&MyMsgCallback);
    ros::spin();
    return 0;
}

服务

PassWord.srv

int64 password
---
bool result

ClientForBeginner.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.27

    Description: Simple ros service client demo for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_service ClientForBeginner 123456
    ****************
*/

#include "ros/ros.h"
#include "book_service/PassWord.h"
#include <cstdlib>

using namespace std;
int main(int argc, char** argv)
{
    //initial and name node
    ros::init(argc,argv,"node_client");
    if(argc<2)
    {
        cout<<"Error paramster,please run eg: rosrun book_service ClientForBeginner 123456"<<endl;
        return 1;
    }
    //create nodehandle
    ros::NodeHandle nh;
    //create client
    ros::ServiceClient client = nh.serviceClient<book_service::PassWord>("pswserver",100);
    book_service::PassWord srv;
    //convert argv from char to int and pass value to service response
    srv.request.password = atoi(argv[1]);
    if(client.call(srv))
    {
        ROS_INFO("client connect success!");
        if(srv.response.result)
        {
            ROS_INFO("Welcom,correct password!");
        }else{
            ROS_INFO("Sorry,password error!");
        }
    }else{
        ROS_INFO("client connet fail!");
    }
    return 0;
}

ServerForBeginner.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.27

    Description: Simple ros service server demo for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_service ServerForBeginner
    ****************
*/

#include "ros/ros.h"
#include "book_service/PassWord.h"

// server callback function
bool serverCallback(book_service::PassWord::Request& req,
                    book_service::PassWord::Response& res)
{
    //if password = 123456, result is true, or result is false
    res.result = (req.password == 123456) ? true:false;
    return true;
}

int main(int argc,char** argv)
{
    //initial and name node
    ros::init(argc,argv,"server_node");
    //create nodehandle
    ros::NodeHandle nh;
    //create service server
    ros::ServiceServer serv = nh.advertiseService("pswserver",&serverCallback);
    ros::spin();
    return 0;
}

参数

book_param.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.27

    Description: Simple ros param demo for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_param book_param 1 
    ****************
*/

#include "ros/ros.h"
#include <cstdlib>

using namespace std;

int main(int argc,char** argv)
{
    //initial and name node
    ros::init(argc,argv,"node_param");
    if(argc!=2)
    {
        cout<<"Error command paramter! Please run command eg:\n"\
            <<"rosrun book_param book_param 1\n"\
            <<"help information:\n"\
            <<" 1 ------ set param mode(ros::param::set())\n"\
            <<" 2 ------ set param mode(ros::NodeHandle::setParam())\n"\
            <<" 3 ------ get param mode(ros::param::get())\n"\
            <<" 4 ------ get param mode(ros::NodeHandle::getParam())\n"\
            <<" 5 ------ get param mode(ros::NodeHandle::param())\n"\
            <<endl;
        return 1;
    }

    //create node handle
    ros::NodeHandle nh;
    //param variable
    int IntParam;
    string StrParam;
    bool isIntParam, isStrParam;
    //mode flag
    int flag = atoi(argv[1]);

    // set or get param with different ways
    switch(flag)
    {
        case 1:
            ROS_INFO("set param mode(ros::param::set()):");
            ros::param::set("IntParam",1);
            ros::param::set("StrParam","stringdemo");
            break;
        case 2:
            ROS_INFO("set param mode(ros::NodeHandle::setParam()):");
            nh.setParam("IntParam",1);
            nh.setParam("StrParam","stringdemo");
            break;
        case 3:
            ROS_INFO("get param mode(ros::param::get()):");
            isIntParam = ros::param::get("IntParam",IntParam);
            isStrParam = ros::param::get("StrParam",StrParam);

            if(isIntParam){
                ROS_INFO("The IntParam is:%d",IntParam);
            }else{
                ROS_INFO("Get IntParam fail!");
            }

            if(isIntParam){
                ROS_INFO("The StrParam is:%s",StrParam.c_str());
            }else{
                ROS_INFO("Get StrParam fail!");
            }
            break;
        case 4:
            ROS_INFO("get param mode(ros::NodeHandle::getParam()):");
            isIntParam = nh.getParam("IntParam",IntParam);
            isStrParam = nh.getParam("StrParam",StrParam);

            if(isIntParam){
                ROS_INFO("The IntParam is:%d",IntParam);
            }else{
                ROS_INFO("Get IntParam fail!");
            }

            if(isIntParam){
                ROS_INFO("The StrParam is:%s",StrParam.c_str());
            }else{
                ROS_INFO("Get StrParam fail!");
            }
            break;
        case 5:
            ROS_INFO("get param mode(ros::NodeHandle::param()):");
            //warning: this way will set default value when get no param!
            nh.param("IntParam",IntParam,11);
            // be careful when use ros::NodeHandle::param get string param!
            nh.param<std::string>("StrParam",StrParam,"stringdemo_default");
            ROS_INFO("\nThe IntParam is:%d\nThe StrParam is:%s",IntParam,StrParam.c_str());
            break;
        default:
            ROS_INFO("flag value is not in range: [1,5]");
    }
    return 0;
}

动态参数设置

DynamicParam.cfg

#!/usr/bin/env python
PACKAGE = "book_dynamic_param"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("IntDyParam",int_t,0,"An Int Parameter",0,0,9)
gen.add("DouDyParam",double_t,0,"A Double Parameter",1.5,0,9)
gen.add("StrDyParam",str_t,0,"A String Parameter","Hello,I'm Robot!")
gen.add("BoolDyParam",bool_t,0,"A Bool Parameter",True)

student_info = gen.enum([gen.const("Name",str_t,"LiLei","Name Information"),
                         gen.const("Sex",str_t,"Man","Sex Information"),
                         gen.const("Age",str_t,"18","Age Information")],
                        "A set contain a student information")

gen.add("StudentInfo",str_t,0,"A studenet information set","LiLei",edit_method=student_info)
exit(gen.generate(PACKAGE,"node_DynamicParam","DynamicParam"))   

book_dyparam.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.28

    Description: Simple ros dynamic parameter demo for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_dynamic_param book_dyparam
              
              rosrun rqt_reconfigure rqt_reconfigure
    ****************
*/

#include "ros/ros.h"
#include "dynamic_reconfigure/server.h"
#include "book_dynamic_param/DynamicParamConfig.h"

//define call back function
void paramCallback(book_dynamic_param::DynamicParamConfig& config,uint32_t level)
{
    ROS_INFO("Request: %d %f %s %s %s",
                config.IntDyParam,config.DouDyParam,
                config.StrDyParam.c_str(),
                config.BoolDyParam?"True":"False",
                config.StudentInfo.c_str());
}

int main(int argc, char** argv)
{
    //initial and name node
    ros::init(argc,argv,"node_DynamicParam");
    //create node handle
    dynamic_reconfigure::Server<book_dynamic_param::DynamicParamConfig> server;
    dynamic_reconfigure::Server<book_dynamic_param::DynamicParamConfig>::CallbackType f;

    f = boost::bind(&paramCallback,_1,_2);
    server.setCallback(f);
    ros::spin();
    return 0;
}

测试

rosrun book_dynamic_param book_dyparam
rosrun rqt_reconfigure rqt_reconfigure

ROS类编程思想

PassWord.srv

int32 password
---
bool result

book_class.h

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Int32.h"
#include "book_class/PassWord.h"
#include <cstdlib>

using namespace std;

class RosBasics
{
    public:
        RosBasics();
        RosBasics(ros::NodeHandle& nh);
        ~RosBasics();
        void initPublish();
        void initSubscribe();
        void initServer();
        bool serverCallback(book_class::PassWord::Request& req,
                            book_class::PassWord::Response& res);
        void msgCallback(const std_msgs::Float64& stdmsg);
        void paramTest();
    private:
        ros::NodeHandle nh_;
        ros::Publisher pub_;
        ros::Subscriber sub_;
        ros::ServiceServer serv_;
};

book_class.cpp

/*
    ****************
    Author: Tomanlon

    Copyright: Tomanlon

    Date: 2019.2.28

    Description: Simple ros class method demo by combining  message ,service and param for beginnner learners

    MailBox: taomanl@163.com

    HowToRun: rosrun book_class book_class
              rostopic pub -r 10 /Topic1 std_msgs/Float64 1 (test message communication)
              rosservice call pwdcheck 123456 (test service communication)
    ****************
*/
#include "book_class.h"

RosBasics::RosBasics():nh_("~")
{
    initPublish();
    initServer();
    initSubscribe();
    paramTest();
}

RosBasics::RosBasics(ros::NodeHandle& nh):nh_(nh)
{
    initPublish();
    initServer();
    initSubscribe();
    paramTest();
}

RosBasics::~RosBasics()
{
}

void RosBasics::initPublish()
{
    ROS_INFO("publish initializing!");
    pub_ = nh_.advertise<std_msgs::Float64>("Topic2",100);
}

void RosBasics::initSubscribe()
{
    ROS_INFO("subscribe initializing!");
    sub_ = nh_.subscribe("Topic1",1,&RosBasics::msgCallback,this);
}

void RosBasics::initServer()
{
    ROS_INFO("server initializing!");
    serv_ = nh_.advertiseService("pwdcheck",&RosBasics::serverCallback,this);
}


bool RosBasics::serverCallback(book_class::PassWord::Request& req,
                               book_class::PassWord::Response& res)
{
    res.result = (req.password == 123456) ? true:false;
    if(res.result)
    {
        ROS_INFO("Welcom, password correct!");
    }else{
        ROS_INFO("Sorry, password error!");
    }
    return true;
}                               


//obtain message from Topic1 and add value then publish to Topic2
void RosBasics::msgCallback(const std_msgs::Float64& stdmsg)
{
    std_msgs::Float64 msgpub;
    msgpub.data = stdmsg.data + 2;
    ROS_INFO("Receive date from Topic1 content is:%f",stdmsg.data);
    ROS_INFO("Publish date with Topic2 content is:%f",msgpub.data);
    pub_.publish(msgpub);
}

//test ros param
void RosBasics::paramTest()
{
    int Age;
    bool checkAge;
    checkAge = nh_.getParam("Age",Age);
    if(checkAge)
    {
        ROS_INFO("Welcome,Get Param Success!");
        if(Age>=18)
        {
            ROS_INFO("You are adult!");
        }else{
            ROS_INFO("You are children!");
        }
    }else{
        ROS_INFO("Sorry,Get Param Fail!");
    }
}

int main(int argc,char** argv)
{
    //initial and name node
    ros::init(argc,argv,"node_classdemo");
    //create node handle
    ros::NodeHandle nh;
    //instantiate class object
    RosBasics classdemo(nh);
    ros::spin();
    return 0;
}

测试

rosrun book_class book_class
rosservice call pwdcheck 123456
rosparam set Age 18
rostopic pub -r 10 /Topic1 std_msgs/Float6  4 1

 

  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
SLAM导航机器人零基础实战系列》讲义 第1章:Linux基础 1.Linux简介 2.安装Linux发行版ubuntu系统 3.Linux命令行基础操作 第2章:ROS入门 1.ROS是什么 2.ROS系统整体架构 3.在ubuntu16.04中安装ROS kinetic 4.如何编写ROS的第一个程序hello_world 5.编写简单的消息发布器和订阅器 6.编写简单的service和client 7.理解tf的原理 8.理解roslaunch在大型项目中的作用 9.熟练使用rviz 10.在实际机器人上运行ROS高级功能预览 第3章:感知与大脑 1.ydlidar-x4激光雷达 2.带自校准九轴数据融合IMU惯性传感器 3.轮式里程计与运动控制 4.音响麦克风与摄像头 5.机器人大脑嵌入式主板性能对比 6.做一个能走路和对话的机器人 第4章:差分底盘设计 1.stm32主控硬件设计 2.stm32主控软件设计 3.底盘通信协议 4.底盘ROS驱动开发 5.底盘PID控制参数整定 6.底盘里程计标 第5章:树莓派3开发环境搭建 1.安装系统ubuntu_mate_16.04 2.安装ros-kinetic 3.装机后一些实用软件安装和系统设置 4.PC端与robot端ROS网络通信 5.Android手机端与robot端ROS网络通信 6.树莓派USB与tty串口号绑定 7.开机自启动ROS节点 第6章:SLAM建图与自主避障导航 1.在机器人上使用传感器 2.google-cartographer机器人SLAM建图 3.ros-navigation机器人自主避障导航 4.多目标点导航及任务调度 5.机器人巡航与现场监控 第7章:语音交互与自然语言处理 1.语音交互相关技术 2.机器人语音交互实现 3.自然语言处理云计算引擎

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值