Boost用法详解原博客链接:https://blog.csdn.net/Windgs_YF/article/details/81201456
https://github.com/Kapernikov/ros_robot_localization_tutorial
odometry_node.cpp
#include <boost/program_options.hpp>
#include <ros/ros.h>
#include <robot_localization_demo/odometry.hpp>
int main(int argc, char ** argv) {
double frequency;
double error_vx_systematic;
double error_vx_random;
double error_wz_systematic;
double error_wz_random;
//命令行或配置文件读取参数选项,设置四个参数X,x,T,t,v。这个例子的参数在launch文件中设置
namespace po = boost::program_options;
po::options_description description("Recognised options");
description.add_options()
("help,h", "print help message")
("frequency,f", po::value<double>(&frequency)->default_value(1.), "set measurement frequency (Hz)")
("error-vx-systematic,X", po::value<double>(&error_vx_systematic)->default_value(0.), "set systematic error on X velocity")
("error-vx-random,x", po::value<double>(&error_vx_random)->default_value(0.), "set random error on X velocity")
("error-wz-systematic,T", po::value<double>(&error_wz_systematic)->default_value(0.), "set systematic error on angular velocity")
("error-wz-random,t", po::value<double>(&error_wz_random)->default_value(0.), "set random error on angular velocity")
("visualize,v", "visualize positioning system measurement");
po::variables_map variables_map;
po::store(po::parse_command_line(argc, argv, description), variables_map);
po::notify(variables_map);
if (variables_map.count("help")) {
std::cout << description << std::endl;
return EXIT_FAILURE;
}
//初始化节点,创建句柄
ros::init(argc, argv, "turtle_odometry");
ros::NodeHandle node_handle;
//有参构造函数
robot_localization_demo::TurtleOdometry turtle_odometry{node_handle, frequency,
error_vx_systematic, error_vx_random, error_wz_systematic, error_wz_random,
(variables_map.count("visualize")? true: false)};
turtle_odometry.spin();
return EXIT_SUCCESS;
}
odometry.cpp
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <tf/transform_datatypes.h>
#include <turtlesim/Spawn.h>
#include <turtlesim/SetPen.h>
#include <turtlesim/TeleportRelative.h>
#include <robot_localization_demo/odometry.hpp>
namespace robot_localization_demo {
TurtleOdometry::TurtleOdometry(ros::NodeHandle node_handle, double frequency,
double error_vx_systematic, double error_vx_random, double error_wz_systematic, double error_wz_random, bool visualize):
node_handle_{node_handle},
turtle_pose_subscriber_{node_handle_.subscribe("turtle1/pose", 16, &TurtleOdometry::turtlePoseCallback, this)},
turtle_twist_publisher_{node_handle_.advertise<geometry_msgs::TwistWithCovarianceStamped>("turtle1/sensors/twist", 16)},
frequency_{frequency},
random_generator_{},
random_distribution_vx_{error_vx_systematic, error_vx_random},
random_distribution_wz_{error_wz_systematic, error_wz_random},
frame_sequence_{0},
visualize_{visualize},
visualization_turtle_name_{""}
{
;
}
TurtleOdometry::~TurtleOdometry() {
;
}
void TurtleOdometry::spin() {
ros::Rate rate(frequency_);
while(node_handle_.ok()) {
ros::spinOnce();
// Distort real twist to get a 'measurement'.
auto measurement = cached_pose_;
measurement.linear_velocity *= (1. + random_distribution_vx_(random_generator_));
measurement.angular_velocity += measurement.linear_velocity * random_distribution_wz_(random_generator_);
// Publish measurement.
geometry_msgs::TwistWithCovarianceStamped current_twist;
current_twist.header.seq = ++ frame_sequence_;
current_twist.header.stamp = ros::Time::now();
current_twist.header.frame_id = "base_link";
current_twist.twist.twist.linear.x = measurement.linear_velocity;
current_twist.twist.twist.linear.y = 0.;
current_twist.twist.twist.linear.z = 0.;
current_twist.twist.twist.angular.x = 0.;
current_twist.twist.twist.angular.y = 0.;
current_twist.twist.twist.angular.z = measurement.angular_velocity;
current_twist.twist.covariance = boost::array<double, 36>({
std::pow(random_distribution_vx_.mean() + random_distribution_vx_.stddev(), 2), 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., std::pow(measurement.linear_velocity * (random_distribution_wz_.mean() + random_distribution_wz_.stddev()), 2)});
turtle_twist_publisher_.publish(current_twist);
if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
moveVisualizationTurtle(measurement);
}
// Sleep until we need to publish a new measurement.
rate.sleep();
}
}
void TurtleOdometry::turtlePoseCallback(const turtlesim::PoseConstPtr & message) {
cached_pose_timestamp_ = ros::Time::now();
cached_pose_ = *message;
// If this is the first message, initialize the visualization turtle.
if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
spawnAndConfigureVisualizationTurtle(*message);
}
}
void TurtleOdometry::spawnAndConfigureVisualizationTurtle(const turtlesim::Pose & initial_pose) {
if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
// Spawn a new turtle and store its name.
ros::service::waitForService("spawn");
turtlesim::Spawn spawn_visualization_turtle;
spawn_visualization_turtle.request.x = initial_pose.x;
spawn_visualization_turtle.request.y = initial_pose.y;
spawn_visualization_turtle.request.theta = initial_pose.theta;
auto client = node_handle_.serviceClient<decltype(spawn_visualization_turtle)>("spawn");
client.call(spawn_visualization_turtle);
visualization_turtle_name_ = spawn_visualization_turtle.response.name;
// Set pen color to blue.
turtlesim::SetPen configure_visualization_turtle;
configure_visualization_turtle.request.r = 255;
configure_visualization_turtle.request.g = 0;
configure_visualization_turtle.request.b = 0;
configure_visualization_turtle.request.width = 1;
configure_visualization_turtle.request.off = 0;
auto client_configure = node_handle_.serviceClient<decltype(configure_visualization_turtle)>(
visualization_turtle_name_ + "/set_pen");
client_configure.call(configure_visualization_turtle);
// Log message.
ROS_INFO("Relative position measurement (odometry) visualized by '%s' with a red pen.", visualization_turtle_name_.c_str());
}
}
void TurtleOdometry::moveVisualizationTurtle(const turtlesim::Pose & measurement) {
if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
// Move visualization turtle to the 'measured' position.
turtlesim::TeleportRelative visualize_current_twist;
visualize_current_twist.request.linear = measurement.linear_velocity / frequency_;
visualize_current_twist.request.angular = measurement.angular_velocity / frequency_;
auto client = node_handle_.serviceClient<decltype(visualize_current_twist)>(
visualization_turtle_name_ + "/teleport_relative");
client.call(visualize_current_twist);
}
}
}
odometry.hpp
#ifndef __robot_localization_demo__odometry__
#define __robot_localization_demo__odometry__
#include <random>
#include <ros/ros.h>
#include <turtlesim/Pose.h>
namespace robot_localization_demo {
//定义一个类,成员函数spin(),有参构造函数和析构函数
class TurtleOdometry {
public:
TurtleOdometry(ros::NodeHandle node_handle, double frequency, double error_vx_systematic, double error_vx_random,
double error_wz_systematic, double error_wz_random, bool visualize=false);
~TurtleOdometry();
void spin();
private:
ros::NodeHandle node_handle_;
ros::Subscriber turtle_pose_subscriber_;
ros::Publisher turtle_twist_publisher_;
double frequency_;
std::default_random_engine random_generator_;
std::normal_distribution<double> random_distribution_vx_;
std::normal_distribution<double> random_distribution_wz_;
bool visualize_;
std::string visualization_turtle_name_;
unsigned frame_sequence_;
ros::Time cached_pose_timestamp_;
turtlesim::Pose cached_pose_;
void turtlePoseCallback(const turtlesim::PoseConstPtr & message);
inline bool isVisualizationRequested() { return visualize_; };
inline bool isVisualizationTurtleAvailable() { return visualization_turtle_name_ != ""; };
void spawnAndConfigureVisualizationTurtle(const turtlesim::Pose & initial_pose);
void moveVisualizationTurtle(const turtlesim::Pose & measurement);
};
}
#endif
launch文件
<launch>
<!-- visualization node to show the real turtle, the measurements and the estimated position -->
<node pkg="turtlesim" type="turtlesim_node" name="turtlesim" />
<!-- keyboard control for the real turtle -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />
<!-- 'turtle positioning system', transforming the position of the real turtle to a noisy measurement at a given frequency -->
<node pkg="robot_localization_demo" type="positioning_system_node" name="turtle1_positioning_system_node"
args="-f 1. -x 0.2 -y 0.2 -t 0.2 -v" output="screen" />
<!-- 'turtle odometry node', transforming the movements of the real turtle to a noisy measurement at a given frequency -->
<node pkg="robot_localization_demo" type="odometry_node" name="turtle1_odometry_node"
args="-f 20. -x 0.05 -X 0. -t 0. -T 0.02 -v" output="screen" />
<!-- robot_localization EKF node for the odom frame -->
<node pkg="robot_localization" type="ekf_localization_node" name="robot_localization_ekf_node_odom" clear_params="true">
<param name="frequency" value="10." />
<param name="sensor_timeout" value="0.2" />
//确保机器人在X-Y平面上
<param name="two_d_mode" value="true" />
<param name="publish_tf" value="true" />
<param name="map_frame" value="map" />
<param name="odom_frame" value="odom" />
<param name="base_link_frame" value="base_link" />
<param name="world_frame" value="odom" />
<param name="print_diagnostics" value="true" />
<remap from="odometry/filtered" to="odometry/filtered_twist" />
<param name="twist0" value="turtle1/sensors/twist" />
<param name="twist0_differential" value="false"/>
<rosparam param="twist0_config">[false, false, false, false, false, false,
true, true, false, false, false, true,
false, false, false]</rosparam>
</node>
<!-- robot_localization EKF node for the map frame -->
<node pkg="robot_localization" type="ekf_localization_node" name="robot_localization_ekf_node_map" clear_params="true">
<param name="frequency" value="10" />
<param name="sensor_timeout" value="0.2" />
<param name="two_d_mode" value="true" />
<param name="publish_tf" value="true" />
<param name="map_frame" value="map" />
<param name="odom_frame" value="odom" />
<param name="base_link_frame" value="base_link" />
<param name="world_frame" value="map" />
<param name="twist0" value="turtle1/sensors/twist" />
<rosparam param="twist0_config">[false, false, false, false, false, false,
true, true, false, false, false, true,
false, false, false]</rosparam>
<param name="pose0" value="turtle1/sensors/pose" />
<rosparam param="pose0_config">[true, true, false, false, false, true,
false, false, false, false, false, false,
false, false, false]</rosparam>
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<!-- transformation visualization node, visualizing the estimated position of the turtle in the map frame -->
<node pkg="robot_localization_demo" type="transformation_visualization_node" name="transformation_visualization_node" />
</launch>