ros c++ 代码说明文档_【ROS教程 004】ROS的调试

#include "ros/ros.h"
 
int main( int argc, char **argv )
{
 
  ros::init( argc, argv, "example1" );
 
  ros::NodeHandle n;
 
  const double val = 3.14;
 
  ROS_DEBUG( "This is a simple DEBUG message!" );
 
  ROS_DEBUG( "This is a DEBUG message with an argument: %f", val );
 
  ROS_DEBUG_STREAM(
  "This is DEBUG stream message with an argument: " << val
  );
 
  ros::spinOnce();
 
  return EXIT_SUCCESS;
 
}

$ roscd davebobo_tutorials/

$ gdb bin/example1

<?xml version="1.0" encoding="UTF-8"?>
 
<launch>
  <!-- Logger config -->
  <env name="ROSCONSOLE_CONFIG_FILE"
  value="$(find chapter3_tutorials)/config/chapter3_tutorials.config"/>
 
  <!-- Example 1 -->
  <node pkg="chapter3_tutorials" type="example1" name="example1"
  output="screen" launch-prefix="xterm -e gdb --args"/>
</launch>

float32 desired_speed
---
float32 previous_speed
float32 new_speed
bool stalled

#include <ros/ros.h>
#include <ros/console.h>
 
#include <std_msgs/Int32.h>
#include <geometry_msgs/Vector3.h>
 
#include <chapter3_tutorials/SetSpeed.h>
 
int main( int argc, char **argv )
{
 
  ros::init( argc, argv, "example4" );
 
  ros::NodeHandle n;
 
  ros::Publisher pub_temp = n.advertise< std_msgs::Int32 >( "temp", 1000 );
  ros::Publisher pub_accel = n.advertise< geometry_msgs::Vector3 >( "accel", 1000 );
 
  ros::ServiceClient srv_speed = n.serviceClient< chapter3_tutorials::SetSpeed>( "speed" );
 
  std_msgs::Int32 msg_temp;
  geometry_msgs::Vector3 msg_accel;
  chapter3_tutorials::SetSpeed msg_speed;
 
  int i = 0;
 
  ros::Rate rate( 1 );
  while( ros::ok() ) {
 
  msg_temp.data = i;
 
  msg_accel.x = 0.1 * i;
  msg_accel.y = 0.2 * i;
  msg_accel.z = 0.3 * i;
 
  msg_speed.request.desired_speed = 0.01 * i;
 
  pub_temp.publish( msg_temp );
  pub_accel.publish( msg_accel );
 
  if( srv_speed.call( msg_speed ) )
  {
  ROS_INFO_STREAM(
  "SetSpeed response:n" <<
  "previous speed = " << msg_speed.response.previous_speed << "n" <<
  "new speed = " << msg_speed.response.new_speed << "n" <<
  "motor stalled = " << ( msg_speed.response.stalled ? "true" : "false" )
  );
  }
  else
  {
  // Note that this might happen at the beginning, because
  // the service server could have not started yet!
  ROS_ERROR_STREAM( "Call to speed service failed!" );
  }
 
  ++i;
 
  ros::spinOnce();
  rate.sleep();
  }
 
  return EXIT_SUCCESS;
 
}

#include <ros/ros.h>
#include <ros/console.h>
 
#include <std_msgs/Int32.h>
#include <geometry_msgs/Vector3.h>
 
#include <chapter3_tutorials/SetSpeed.h>
 
float previous_speed = 0.;
float new_speed = 0.;
 
void callback_temp( const std_msgs::Int32::ConstPtr& msg )
{
  ROS_INFO_STREAM( "Temp = " << msg->data );
}
 
void callback_accel( const geometry_msgs::Vector3::ConstPtr& msg )
{
  ROS_INFO_STREAM(
  "Accel = (" << msg->x << ", " << msg->y << ", " << msg->z << ")"
  );
}
 
bool callback_speed(
  chapter3_tutorials::SetSpeed::Request &req,
  chapter3_tutorials::SetSpeed::Response &res
)
{
  ROS_INFO_STREAM(
  "speed service request: desired speed = " << req.desired_speed
  );
 
  new_speed = 0.9 * req.desired_speed;
 
  res.previous_speed = previous_speed;
  res.new_speed = new_speed;
  res.stalled = new_speed < 0.1;
 
  previous_speed = new_speed;
 
  return true;
}
 
int main( int argc, char **argv )
{
 
  ros::init( argc, argv, "example5" );
 
  ros::NodeHandle n;
 
  ros::Subscriber sub_temp = n.subscribe( "temp", 1000, callback_temp );
  ros::Subscriber sub_accel = n.subscribe( "accel", 1000, callback_accel );
 
  ros::ServiceServer srv_speed = n.advertiseService( "speed", callback_speed );
 
  while( ros::ok() ) {
  ros::spin();
  }
 
  return EXIT_SUCCESS;
 
}

<?xml version="1.0" encoding="UTF-8"?>
 
<launch>
  <!-- Example 4 and 5 -->
  <node pkg="chapter3_tutorials" type="example4" name="example4"
  output="screen"/>
  <node pkg="chapter3_tutorials" type="example5" name="example5"
  output="screen"/>
</launch>

rosbuild_add_executable(example4 src/example4.cpp)
rosbuild_add_executable(example5 src/example5.cpp)

$ roslaunch chapter3_tutorials example4_5.launch

$ rxgraph --