ROS----示例

Posted 2021-03-06 feihu-h

一、代码

 

 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_;
};

bc.cpp

#include "book_class.h"
///////////////////////////////////////////////////////////run////////////////////////udisks2
////////////////////////////////////////////////////////

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

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

RosBasics::~RosBasics()
{
}

/**
  定义主题: "Topic2"
           消息类型：std_msgs::Float64
  发布端
*/
void RosBasics::initPublish()
{
    ROS_INFO("publish initializing!");
    pub_ = nh_.advertise<std_msgs::Float64>("Topic2",100);
}

/**
    订阅主题 "Topic1"
    回调函数：RosBasics::msgCallback
*/
void RosBasics::initSubscribe()
{
    ROS_INFO("subscribe initializing!");
    sub_ = nh_.subscribe("Topic1",1,&RosBasics::msgCallback,this);
}

/**
    请求服务： 服务端  "pwdcheck"
    回调函数：&RosBasics::serverCallback
*/
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;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(book_class)


find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  message_generation
)

add_service_files(FILES
  PassWord.srv
)


generate_messages(DEPENDENCIES
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS
  message_runtime
#  INCLUDE_DIRS include
#  LIBRARIES book_class
#  CATKIN_DEPENDS other_catkin_pkg
#  DEPENDS system_lib
)

include_directories(
  include ${catkin_INCLUDE_DIRS}
# include
# ${catkin_INCLUDE_DIRS}
)




add_executable(bc
  src/bc.cpp
)
add_dependencies(bc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(bc
  ${catkin_LIBRARIES}
)

二、测试

rosrun book_class bc

 

rostopic pub -r 10  /Topic1  std_msgs/Float64  1

rosservice  call   pwdcheck   123456

rosparam   set  Age  18

rosrun rqt_graph   rqt_graph