/*
* Read parameter(s) and set system parameters.
*/
void Perturbation::readParameters(std::istream& in)
{
read<int>(in, "mode", mode_);
read<int>(in, "nParameters", nParameters_);
parameters_.allocate(size_, nParameters_);
parameter_.allocate(nParameters_);
initialParameter_.allocate(nParameters_);
finalParameter_.allocate(nParameters_);
if (mode_ == 0) {
readDMatrix<double>(in, "parameters", parameters_, size_, nParameters_);
for (int i = 0; i < nParameters_; ++i) {
parameter_[i] = parameters_(rank_,i);
}
} else if (mode_ == 1) {
readDArray<double>(in, "initialParameter", initialParameter_, nParameters_);
readDArray<double>(in, "finalParameter", finalParameter_, nParameters_);
int i, j;
for (i = 0; i < nParameters_; ++i) {
parameters_(0,i) = initialParameter_[i];
parameters_(size_-1,i) = finalParameter_[i];
for (j= 1; j < size_-1; ++j) {
parameters_(j, i) = parameters_(0, i)
+ j*((parameters_(size_-1, i) - parameters_(0, i))/(size_-1));
}
}
for (i = 0; i < nParameters_; ++i) {
parameter_[i] = parameters_(rank_, i);
}
}
setParameter(); // Modify parameter of associated System
}
void Node::onInit()
{
// map_size parameter
XmlRpc::XmlRpcValue p_map_size;
if (getPrivateNodeHandle().getParam("map_size", p_map_size)) {
Size &map_size = parameters_("map_size", Size(0, 0, 0));
if (p_map_size.getType() == XmlRpc::XmlRpcValue::TypeInt) {
map_size.x() = map_size.y() = static_cast<int>(p_map_size);
} else if (p_map_size.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_size.size() >= 2) {
map_size.x() = static_cast<int>(p_map_size[0]);
map_size.y() = static_cast<int>(p_map_size[1]);
}
if (p_map_size.size() >= 3) {
map_size.z() = static_cast<int>(p_map_size[2]);
}
}
}
// map_resolution parameter
XmlRpc::XmlRpcValue p_map_resolution;
if (getPrivateNodeHandle().getParam("map_resolution", p_map_resolution)) {
Resolution &resolution = parameters_("map_resolution", Resolution(0.0, 0.0, 0.0));
if (p_map_resolution.getType() == XmlRpc::XmlRpcValue::TypeDouble) {
resolution.x() = resolution.y() = static_cast<double>(p_map_resolution);
} else if (p_map_resolution.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_resolution.size() >= 2) {
resolution.x() = static_cast<double>(p_map_resolution[0]);
resolution.y() = static_cast<double>(p_map_resolution[1]);
}
if (p_map_resolution.size() >= 3) {
resolution.z() = static_cast<double>(p_map_resolution[2]);
}
}
}
// map_offset parameter
XmlRpc::XmlRpcValue p_map_offset;
if (getPrivateNodeHandle().getParam("map_offset", p_map_offset)) {
Point &offset = parameters_("map_offset", Point(0.0, 0.0, 0.0));
if (p_map_offset.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_offset.size() >= 2) {
offset.x() = static_cast<double>(p_map_offset[0]);
offset.y() = static_cast<double>(p_map_offset[1]);
}
if (p_map_offset.size() >= 3) {
offset.z() = static_cast<double>(p_map_offset[2]);
}
}
}
// map_type parameter
std::string p_map_type = "OccupancyGridMap2D";
getPrivateNodeHandle().getParam("map_type", p_map_type);
map_ = MapFactory(parameters_).create<OccupancyGridMapBase>(p_map_type);
if (!map_) {
ROS_FATAL("Unknown map type: %s.\n\nAvailable map types:\n%s", p_map_type.c_str(), MapFactory::getMapTypes().c_str());
ros::shutdown();
return;
}
// scan matcher parameters
ScanMatcherParameters &matcher_parameters = parameters_("matcher", ScanMatcherParameters());
getPrivateNodeHandle().getParam("match_level_minimum", matcher_parameters.match_level_minimum());
getPrivateNodeHandle().getParam("match_level_maximum", matcher_parameters.match_level_maximum());
getPrivateNodeHandle().getParam("occupied_space_residual_weight", matcher_parameters.occupied_space_residual_weight());
getPrivateNodeHandle().getParam("free_space_residual_weight", matcher_parameters.free_space_residual_weight());
getPrivateNodeHandle().getParam("motion_residual_weight", matcher_parameters.motion_residual_weight());
getPrivateNodeHandle().getParam("function_tolerance", matcher_parameters.function_tolerance());
getPrivateNodeHandle().getParam("gradient_tolerance", matcher_parameters.gradient_tolerance());
getPrivateNodeHandle().getParam("parameter_tolerance", matcher_parameters.parameter_tolerance());
getPrivateNodeHandle().getParam("max_num_iterations", matcher_parameters.max_num_iterations());
getPrivateNodeHandle().getParam("max_solver_time_in_seconds", matcher_parameters.max_solver_time_in_seconds());
// get occupancy parameters
OccupancyParameters &occupancy_parameters = parameters_("occupancy", OccupancyParameters::Default());
double p_update_factor_free, p_update_factor_occupied;
// private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
// private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
if (getPrivateNodeHandle().getParam("update_factor_free", p_update_factor_free))
occupancy_parameters.step_free() = occupancy_parameters.getOccupancy(p_update_factor_free);
if (getPrivateNodeHandle().getParam("update_factor_occupied", p_update_factor_occupied))
occupancy_parameters.step_occupied() = occupancy_parameters.getOccupancy(p_update_factor_occupied);
// get scan parameters
ScanParameters &scan_parameters = parameters_("scan", ScanParameters());
getPrivateNodeHandle().getParam("laser_min_dist", scan_parameters.min_distance());
getPrivateNodeHandle().getParam("laser_max_dist", scan_parameters.max_distance());
getPrivateNodeHandle().getParam("laser_z_min_value", scan_parameters.min_z());
getPrivateNodeHandle().getParam("laser_z_max_value", scan_parameters.max_z());
// get other parameters
getPrivateNodeHandle().getParam("map_frame", p_map_frame_);
getPrivateNodeHandle().getParam("base_frame", p_base_frame_);
getPrivateNodeHandle().getParam("odom_frame", p_odom_frame_);
getPrivateNodeHandle().getParam("use_tf_scan_transformation", p_use_tf_scan_transformation_);
getPrivateNodeHandle().getParam("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_);
getPrivateNodeHandle().getParam("pub_map_odom_transform", p_pub_map_odom_transform_);
getPrivateNodeHandle().getParam("advertise_map_service", p_advertise_map_service_);
getPrivateNodeHandle().getParam("map_update_distance_thresh", p_map_update_translational_threshold_);
getPrivateNodeHandle().getParam("map_update_angle_thresh", p_map_update_angular_threshold_);
// private_nh_.param("pub_drawings", p_pub_drawings, false);
// private_nh_.param("pub_debug_output", p_pub_debug_output_, false);
// private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true);
// private_nh_.param("pub_odometry", p_pub_odometry_,false);
// private_nh_.param("advertise_map_service", p_advertise_map_service_,true);
// private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5);
// private_nh_.param("map_resolution", p_map_resolution_, 0.025);
// private_nh_.param("map_size", p_map_size_, 1024);
// private_nh_.param("map_start_x", p_map_start_x_, 0.5);
// private_nh_.param("map_start_y", p_map_start_y_, 0.5);
// private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3);
// private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
// private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
// private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4);
// private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9);
// private_nh_.param("scan_topic", p_scan_topic_, std::string("scan"));
// private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand"));
// private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate"));
// private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true);
// private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false);
// private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false);
// private_nh_.param("base_frame", p_base_frame_, std::string("base_link"));
// private_nh_.param("map_frame", p_map_frame_, std::string("map"));
// private_nh_.param("odom_frame", p_odom_frame_, std::string("odom"));
// private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true);
// private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_, std::string("scanmatcher_frame"));
// private_nh_.param("output_timing", p_timing_output_,false);
// private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
// double tmp = 0.0;
// private_nh_.param("laser_min_dist", tmp, 0.4);
// p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);
// private_nh_.param("laser_max_dist", tmp, 30.0);
// p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);
// private_nh_.param("laser_z_min_value", tmp, -1.0);
// p_laser_z_min_value_ = static_cast<float>(tmp);
// private_nh_.param("laser_z_max_value", tmp, 1.0);
// p_laser_z_max_value_ = static_cast<float>(tmp);
// initialize scan and scan matcher
if (p_use_tf_scan_transformation_) scan_.setTransformer(getTransformListener(), p_base_frame_);
matcher_ = ScanMatcher::Factory(parameters_);
// subscribe scan
scan_subscriber_ = getNodeHandle().subscribe<sensor_msgs::LaserScan>("scan", 10, &Node::scanCallback, this);
cloud_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>("point_cloud", 10, &Node::cloudCallback, this);
// initial pose subscriber
initial_pose_subscriber_ = getNodeHandle().subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10, &Node::initialPoseCallback, this);
// static map subscriber
static_map_subscriber_ = getNodeHandle().subscribe<nav_msgs::OccupancyGrid>("static_map", 10, &Node::staticMapCallback, this);
// subscribe syscommand (reset)
syscommand_subscriber_ = getNodeHandle().subscribe<std_msgs::String>("syscommand", 10, &Node::syscommandCallback, this);
// advertise map
map_publisher_ = getNodeHandle().advertise<nav_msgs::OccupancyGrid>("map", 1);
map_metadata_publisher_ = getNodeHandle().advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ROS_INFO("Advertised map as %s", map_publisher_.getTopic().c_str());
// advertise map service
if (p_advertise_map_service_) {
map_service_ = getNodeHandle().advertiseService("map", &Node::mapServiceCallback, this);
}
// advertise pose
pose_with_covariance_publisher_ = getNodeHandle().advertise<geometry_msgs::PoseWithCovarianceStamped>("poseupdate", 1);
pose_publisher_ = getPrivateNodeHandle().advertise<geometry_msgs::PoseStamped>("pose", 1);
covariance_publisher_ = getPrivateNodeHandle().advertise<visualization_msgs::Marker>("covariance", 1);
// advertise tf
if (p_pub_map_odom_transform_) {
getTransformListener();
getTransformBroadcaster();
}
// advertise scan cloud
bool p_publish_scan_cloud = true;
getPrivateNodeHandle().getParam("publish_scan_cloud", p_publish_scan_cloud);
if (p_publish_scan_cloud) scan_.advertisePointCloud(getPrivateNodeHandle());
// setup map publish thread
double p_map_publish_period = 1.0;
getPrivateNodeHandle().getParam("map_publish_period", p_map_publish_period);
if (p_map_publish_period > 0.0) {
map_publish_thread_ = boost::thread(boost::bind(&Node::mapPublishThread, this, ros::Rate(ros::Duration(p_map_publish_period))));
}
// advertise timing information
#ifdef USE_HECTOR_TIMING
timing_publisher_ = getPrivateNodeHandle().advertise<hector_diagnostics::TimingInfo>("timing", 1);
#endif
// reset
reset();
}
/*
* Get parameter i of system id.
*/
double Perturbation::parameter(int i, int id)
{ return parameters_(id,i); }
Node::Node()
: scan_(parameters_)
{
parameters_("map_size", Size(1024, 1024, 1024));
parameters_("map_resolution", Resolution(0.05, 0.05, 0.05));
parameters_("map_frame", p_map_frame_ = "map");
parameters_("base_frame", p_base_frame_ = "base_link");
parameters_("odom_frame", p_odom_frame_ = "base_link");
parameters_("use_tf_scan_transformation", p_use_tf_scan_transformation_ = true);
parameters_("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_ = false);
parameters_("pub_map_odom_transform_", p_pub_map_odom_transform_ = true);
parameters_("advertise_map_service", p_advertise_map_service_ = true);
parameters_("map_update_distance_thresh", p_map_update_translational_threshold_ = 0.4);
parameters_("map_update_angle_thresh", p_map_update_angular_threshold_ = 0.9);
}
