ROS示例代码(c++)

Publisher & Subscriber

Publisher:

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

#include <sstream>


int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");

  ros::NodeHandle n;

  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

  ros::Rate loop_rate(10);

  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
    ROS_INFO("%s", msg.data.c_str());

    chatter_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }

  return 0;
}

Subscriber

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


void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");


  ros::NodeHandle n;


  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);


  ros::spin();

  return 0;
}

Service & Client

Service

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

Client

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

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  if (argc != 3)
  {
    ROS_INFO("usage: add_two_ints_client X Y");
    return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
  beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

Parameters

getParam()

bool getParam (const std::string& key, parameter_type& output_value) const

# 示例
std::string s;
n.getParam("my_param", s);

std::string s;
if (n.getParam("my_param", s))
{
  ROS_INFO("Got param: %s", s.c_str());
}
else
{
  ROS_ERROR("Failed to get param 'my_param'");
}

param()

# 示例
int i;
n.param("my_num", i, 42);

std::string s;
n.param<std::string>("my_param", s, "default_value");

setParam()

n.setParam("my_param", "hello there");

deleteParam()

n.deleteParam("my_param");

hasParam()

if (!n.hasParam("my_param"))
{
  ROS_INFO("No param named 'my_param'");
}

searchParam()

std::string param_name;
if (n.searchParam("b", param_name))
{
  // Found parameter, can now query it using param_name
  int i = 0;
  n.getParam(param_name, i);
}
else
{
  ROS_INFO("No param 'b' found in an upward search");
}

Accessing Private Names

ros::init(argc, argv, "my_node_name");
ros::NodeHandle nh1("~");  // must be in main()
ros::NodeHandle nh2("~foo");

nh1的命名空间为/my_node_name,nh2的命名空间为/my_node_name/foo。

ros::NodeHandle nh("~");
nh.getParam("name", ... );

Class Methods Callbacks

Publisher & Subscriber

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


class Listener
{
public:
  void callback(const std_msgs::String::ConstPtr& msg);
};

void Listener::callback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener_class");
  ros::NodeHandle n;


  Listener listener;
  ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);

  ros::spin();

  return 0;
}

Service & Client

#include "ros/ros.h"
#include "roscpp_tutorials/TwoInts.h"


class AddTwo
{
public:
  bool add(roscpp_tutorials::TwoInts::Request& req,
           roscpp_tutorials::TwoInts::Response& res);
};

bool AddTwo::add(roscpp_tutorials::TwoInts::Request& req,
                 roscpp_tutorials::TwoInts::Response& res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("  sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;


  AddTwo a;
  ros::ServiceServer ss = n.advertiseService("add_two_ints", &AddTwo::add, &a);

  ros::spin();

  return 0;
}

Timers

#include "ros/ros.h"

void callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}

void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;


  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);

  ros::spin();

  return 0;
}

发表评论

您的电子邮箱地址不会被公开。

此站点使用Akismet来减少垃圾评论。了解我们如何处理您的评论数据