ROS教程第六步

最新推荐文章于 2024-01-27 00:58:26 发布

Micozzc_lyy

最新推荐文章于 2024-01-27 00:58:26 发布

阅读量177

点赞数

分类专栏： ROS 文章标签： ros

本文链接：https://blog.csdn.net/weixin_43822768/article/details/90740474

版权

ROS 专栏收录该内容

15 篇文章 0 订阅

订阅专栏

编写复杂的节点——类的方式

1. 节点文件

#include"ros/ros.h"
#include"geometry_msgs/Twist.h"
#include"formcar/carinfo.h"
#include<iostream>
#include<string>
#include<vector>
#include<algorithm>
using namespace std;

struct Carpos{
    float px;
    float py;
};

struct Carinfo{
   float linear_x;
   float angular_z;
};

class Control{
 public:
       Control(int n):hz(n){
         ctrl_x = 1.0;    // 控制量
         ctrl_z = 1.0;
         car_x = 1.0;     // 状态量
         car_y = 1.0;
         count = 0;
         //recar1pos = {0.0,0.0};
        // recar1info = {0.0,0.0};
       }
  void run(){
        ros::NodeHandle n;
        ros::Rate loop(hz); // 创建延时函数
        pub_carpos = n.advertise<formcar::carinfo>("/carmsg",5);
        pub_carctrl = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",5);
        sub_carpos = n.subscribe("/carmsg",5,&Control::Recarpos,this);
        sub_carctrl = n.subscribe("/turtle1/cmd_vel",5,&Control::Recarctrl,this);  // 类名字+函数名+this ,变为静态函数
     
     while (ros::ok()){
       
        // 发布消息数据更新
        
        carpos_msg.px = car_x;
        carpos_msg.py = car_y;
        
        carctrl_msg.linear.x = ctrl_x;
        carctrl_msg.angular.z = ctrl_z;
        // 发布消息
        pub_carpos.publish(carpos_msg);
        pub_carctrl.publish(carctrl_msg);
        ROS_INFO("Send_Message: px = %f py = %f linear_x = %f angular_z = %f [Count = %d]",carpos_msg.px,carpos_msg.py,carctrl_msg.linear.x,carctrl_msg.angular.z,count);
        ROS_INFO("Receive_Message: px = %f py = %f linear_x = %f angular_z = %f [Count = %d]",recar1pos.px,recar1pos.py,recar1info.linear_x,recar1info.angular_z,count);
        ROS_INFO("  ");
        // 数据累加
        car_x += 0.1;
        car_y += 0.1;
        ctrl_x += 0.1;
        ctrl_z += 0.1;
        ++count;
        ros::spinOnce(); // 一次一次接收消息
        loop.sleep();
    }

  }


private:
  geometry_msgs::Twist carctrl_msg;
  geometry_msgs::Twist recarctrl_msg;
  formcar::carinfo carpos_msg;
  formcar::carinfo recarpos_msg; 
  int hz;
  float ctrl_x;
  float ctrl_z;
  float car_x;
  float car_y;
  int count;
  ros::Publisher pub_carpos;
  ros::Publisher pub_carctrl;
  ros::Subscriber sub_carpos;
  ros::Subscriber sub_carctrl;
  Carpos recar1pos;
  Carinfo recar1info;

 void Recarpos(const formcar::carinfo& msg){
     recar1pos.px = msg.px;
     recar1pos.py = msg.py; 
   }

 void Recarctrl(const geometry_msgs::Twist& msg){
     recar1info.linear_x = msg.linear.x;
     recar1info.angular_z = msg.angular.z;
  }  
   
};


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

    ros::init(argc,argv,"thefirstcar");
    Control sample(10);
    sample.run();
}

2. 消息文件

float64 px
float64 py
float64 vx
float64 vy
float64 ux
float64 uy

3. CmakeList.txt文件

cmake_minimum_required(VERSION 2.8.3)
project(formcar)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  geometry_msgs
  message_generation
)

 add_message_files(
   FILES
#   Message1.msg
#   Message2.msg
   carinfo.msg
 )

generate_messages(
   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
  geometry_msgs
 )

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES formcar
#  CATKIN_DEPENDS roscpp rospy
#  DEPENDS system_lib
 CATKIN_DEPENDS message_runtime
 geometry_msgs
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

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

4. package文件

<?xml version="1.0"?>
<package format="2">
  <name>formcar</name>
  <version>0.0.0</version>
  <description>The formcar package</description>
  <maintainer email="macl@todo.todo">macl</maintainer>
  
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <exec_depend>geometry_msgs</exec_depend>

  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>