VisualizeDetectedObjects::VisualizeDetectedObjects() : arrow_height_(0.5), label_height_(1.0)
{
ros::NodeHandle private_nh_("~");
ros_namespace_ = ros::this_node::getNamespace();
if (ros_namespace_.substr(0, 2) == "//")
{
ros_namespace_.erase(ros_namespace_.begin());
}
std::string object_src_topic = ros_namespace_ + "/objects";
std::string markers_out_topic = ros_namespace_ + "/objects_markers";
private_nh_.param<double>("object_speed_threshold", object_speed_threshold_, 0.1);
ROS_INFO("[%s] object_speed_threshold: %.2f", __APP_NAME__, object_speed_threshold_);
private_nh_.param<double>("arrow_speed_threshold", arrow_speed_threshold_, 0.25);
ROS_INFO("[%s] arrow_speed_threshold: %.2f", __APP_NAME__, arrow_speed_threshold_);
private_nh_.param<double>("marker_display_duration", marker_display_duration_, 0.2);
ROS_INFO("[%s] marker_display_duration: %.2f", __APP_NAME__, marker_display_duration_);
std::vector<double> color;
private_nh_.param<std::vector<double>>("label_color", color, {255.,255.,255.,1.0});
label_color_ = ParseColor(color);
ROS_INFO("[%s] label_color: %s", __APP_NAME__, ColorToString(label_color_).c_str());
private_nh_.param<std::vector<double>>("arrow_color", color, {0.,255.,0.,0.8});
arrow_color_ = ParseColor(color);
ROS_INFO("[%s] arrow_color: %s", __APP_NAME__, ColorToString(arrow_color_).c_str());
private_nh_.param<std::vector<double>>("hull_color", color, {51.,204.,51.,0.8});
hull_color_ = ParseColor(color);
ROS_INFO("[%s] hull_color: %s", __APP_NAME__, ColorToString(hull_color_).c_str());
private_nh_.param<std::vector<double>>("box_color", color, {51.,128.,204.,0.8});
box_color_ = ParseColor(color);
ROS_INFO("[%s] box_color: %s", __APP_NAME__, ColorToString(box_color_).c_str());
private_nh_.param<std::vector<double>>("model_color", color, {190.,190.,190.,0.5});
model_color_ = ParseColor(color);
ROS_INFO("[%s] model_color: %s", __APP_NAME__, ColorToString(model_color_).c_str());
private_nh_.param<std::vector<double>>("centroid_color", color, {77.,121.,255.,0.8});
centroid_color_ = ParseColor(color);
ROS_INFO("[%s] centroid_color: %s", __APP_NAME__, ColorToString(centroid_color_).c_str());
subscriber_detected_objects_ =
node_handle_.subscribe(object_src_topic, 1,
&VisualizeDetectedObjects::DetectedObjectsCallback, this);
ROS_INFO("[%s] object_src_topic: %s", __APP_NAME__, object_src_topic.c_str());
publisher_markers_ = node_handle_.advertise<visualization_msgs::MarkerArray>(
markers_out_topic, 1);
ROS_INFO("[%s] markers_out_topic: %s", __APP_NAME__, markers_out_topic.c_str());
}
SlamKarto::SlamKarto() :
got_map_(false),
laser_count_(0),
transform_thread_(NULL),
marker_count_(0)
{
map_to_odom_.setIdentity();
// Retrieve parameters
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))
tmp = 5.0;
map_update_interval_.fromSec(tmp);
if(!private_nh_.getParam("resolution", resolution_))
{
// Compatibility with slam_gmapping, which uses "delta" to mean
// resolution
if(!private_nh_.getParam("delta", resolution_))
resolution_ = 0.05;
}
double transform_publish_period;
private_nh_.param("transform_publish_period", transform_publish_period, 0.05);
// Set up advertisements and subscriptions
tfB_ = new tf::TransformBroadcaster();
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamKarto::laserCallback, this, _1));
marker_publisher_ = node_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",1);
// Create a thread to periodically publish the latest map->odom
// transform; it needs to go out regularly, uninterrupted by potentially
// long periods of computation in our main loop.
transform_thread_ = new boost::thread(boost::bind(&SlamKarto::publishLoop, this, transform_publish_period));
// Initialize Karto structures
mapper_ = new karto::Mapper();
dataset_ = new karto::Dataset();
// Set solver to be used in loop closure
solver_ = new SpaSolver();
mapper_->SetScanSolver(solver_);
}
pr2_r_gripper_control_pub()
{
ros::NodeHandle private_nh_("~");
private_nh_.param("open_position", open_cmd_.position, 0.08);
private_nh_.param("open_max_effort", open_cmd_.max_effort, -1.0);
private_nh_.param("close_position", close_cmd_.position, -100.00);
private_nh_.param("close_max_effort", close_cmd_.max_effort, -1.0);
pub_ = nh_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("r_gripper_controller/command", 1, false);
value = 1;
}
void MoveSlowAndClear::initialize (std::string n, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* global_costmap,
costmap_2d::Costmap2DROS* local_costmap)
{
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
ros::NodeHandle private_nh_ ("~/" + n);
private_nh_.param ("clearing_distance", clearing_distance_, 0.5);
private_nh_.param ("limited_trans_speed", limited_trans_speed_, 0.25);
private_nh_.param ("limited_rot_speed", limited_rot_speed_, 0.45);
private_nh_.param ("limited_distance", limited_distance_, 0.3);
std::string planner_namespace;
private_nh_.param ("planner_namespace", planner_namespace, std::string ("DWAPlannerROS"));
planner_nh_ = ros::NodeHandle ("~/" + planner_namespace);
planner_dynamic_reconfigure_service_ = planner_nh_.serviceClient<dynamic_reconfigure::Reconfigure> ("set_parameters", true);
initialized_ = true;
}
// Constructor
NodeClass()
{
// create a handle for this node, initialize node
ros::NodeHandle private_nh_("~");
// fill Axis handle
//Axis = new CANOpenMaster();
// global variables
SYNC_Freq_HZ = 100; //Looprate for SYNC commands in Hz
// msg
msg_Error.name = "CANError";
// init topic publisher
topicPub_CANError = n.advertise<diagnostic_msgs::DiagnosticStatus>("CANError",1);
// init topic subscriber
}
void ImageProcessing::loadParameters()
{
ros::NodeHandle private_nh_("~");
//! int surf_threshold;
private_nh_.param<int>("surf_threshold",parameters_.surf_threshold_,100);
//! string image_topic;
private_nh_.param<string>("image_topic",parameters_.image_topic_,"/image_raw");
private_nh_.param<string>("sonar_topic",parameters_.sonar_topic_,"/sonar");
//! string image_transport;
private_nh_.param<string>("image_transport",parameters_.image_transport_,"raw");
//! string image_transport;
private_nh_.param<string>("sonar_transport",parameters_.sonar_transport_,"raw");
//! string output_topic;
private_nh_.param<string>("descriptors_topic",parameters_.descriptors_topic_,"/descriptors");
//! string output_topic;
private_nh_.param<string>("keypoints_topic",parameters_.image_keypoints_topic_,"/image_keypoints");
//! int frames_to_jump;
private_nh_.param<int>("frames_to_jump",parameters_.frames_to_jump_,0);
private_nh_.param<string>("source",parameters_.source_,"camera");
private_nh_.param<string>("sonar_mask",parameters_.sonar_mask_,"sonar_mask.jpg");
private_nh_.param<string>("image_detector",parameters_.image_detector_,"surf");
private_nh_.param<bool>("apply_roi",parameters_.apply_roi_,false);
private_nh_.param<bool>("use_selected_images",parameters_.use_selected_images_,false);
private_nh_.param<string>("selected_images_file",parameters_.selected_images_file_,"good_images.txt");
}
// Constructor
NodeClass()
{
// create a handle for this node, initialize node
ros::NodeHandle private_nh_("~");
// initialize global variables
private_nh_.param<std::string>("port", port, "/dev/ttyUSB0");
private_nh_.param<int>("baud", baud, 500000);
private_nh_.param<bool>("inverted", inverted, false);
private_nh_.param<std::string>("frame_id", frame_id, "/base_laser_link");
private_nh_.param<int>("start_scan", start_scan, 115);
private_nh_.param<int>("stop_scan", stop_scan, 426);
// implementation of topics to publish
topicPub_LaserScan = nodeHandle.advertise<sensor_msgs::LaserScan>("scan", 1);
// implementation of topics to subscribe
//--
// implementation of service servers
//--
}
AstarSearch::AstarSearch()
: node_initialized_(false)
{
ros::NodeHandle private_nh_("~");
private_nh_.param<bool>("use_2dnav_goal", use_2dnav_goal_, true);
private_nh_.param<std::string>("path_frame", path_frame_, "map");
private_nh_.param<int>("angle_size", angle_size_, 40);
private_nh_.param<double>("minimum_turning_radius", minimum_turning_radius_, 5.1);
private_nh_.param<int>("obstacle_threshold", obstacle_threshold_, 15);
private_nh_.param<double>("goal_radius", goal_radius_, 0.15);
private_nh_.param<double>("goal_angle", goal_angle_, 6.0);
private_nh_.param<bool>("use_back", use_back_, true);
private_nh_.param<double>("robot_length", robot_length_, 4.5);
private_nh_.param<double>("robot_width", robot_width_, 1.75);
private_nh_.param<double>("base2back", base2back_, 0.8);
private_nh_.param<double>("curve_weight", curve_weight_, 1.05);
private_nh_.param<double>("reverse_weight", reverse_weight_, 2.00);
private_nh_.param<bool>("use_wavefront_heuristic", use_wavefront_heuristic_, true);
private_nh_.param<double>("reverse_weight", reverse_weight_, 2.00);
createStateUpdateTable(angle_size_);
}
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
laser_frame_ = scan.header.frame_id;
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;
ident.stamp_ = scan.header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
// create a point 1m above the laser position and transform it into the laser-frame
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
base_frame_);
try
{
tf_.transformPoint(laser_frame_, up, up);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to determine orientation of laser: %s",
e.what());
return false;
}
// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
if (fabs(fabs(up.z()) - 1) > 0.001)
{
ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
up.z());
return false;
}
gsp_laser_beam_count_ = scan.ranges.size();
int orientationFactor;
if (up.z() > 0)
{
orientationFactor = 1;
ROS_INFO("Laser is mounted upwards.");
}
else
{
orientationFactor = -1;
ROS_INFO("Laser is mounted upside down.");
}
angle_min_ = orientationFactor * scan.angle_min;
angle_max_ = orientationFactor * scan.angle_max;
gsp_laser_angle_increment_ = orientationFactor * scan.angle_increment;
ROS_DEBUG("Laser angles top down in laser-frame: min: %.3f max: %.3f inc: %.3f", angle_min_, angle_max_, gsp_laser_angle_increment_);
GMapping::OrientedPoint gmap_pose(0, 0, 0);
// setting maxRange and maxUrange here so we can set a reasonable default
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(gsp_laser_angle_increment_),
gmap_pose,
0.0,
maxRange_);
ROS_ASSERT(gsp_laser_);
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);
/// @todo Expose setting an initial pose
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,time(NULL));
ROS_INFO("Initialization complete");
return true;
}
SlamGMapping::SlamGMapping():
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0), transform_thread_(NULL)
{
// log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
// The library is pretty chatty
//gsp_ = new GMapping::GridSlamProcessor(std::cerr);
gsp_ = new GMapping::GridSlamProcessor();
ROS_ASSERT(gsp_);
tfB_ = new tf::TransformBroadcaster();
ROS_ASSERT(tfB_);
gsp_laser_ = NULL;
gsp_laser_angle_increment_ = 0.0;
gsp_odom_ = NULL;
got_first_scan_ = false;
got_map_ = false;
ros::NodeHandle private_nh_("~");
// Parameters used by our GMapping wrapper
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
double transform_publish_period;
private_nh_.param("transform_publish_period", transform_publish_period, 0.05);
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))
tmp = 5.0;
map_update_interval_.fromSec(tmp);
// Parameters used by GMapping itself
maxUrange_ = 0.0; maxRange_ = 0.0; // preliminary default, will be set in initMapper()
if(!private_nh_.getParam("sigma", sigma_))
sigma_ = 0.05;
if(!private_nh_.getParam("kernelSize", kernelSize_))
kernelSize_ = 1;
if(!private_nh_.getParam("lstep", lstep_))
lstep_ = 0.05;
if(!private_nh_.getParam("astep", astep_))
astep_ = 0.05;
if(!private_nh_.getParam("iterations", iterations_))
iterations_ = 5;
if(!private_nh_.getParam("lsigma", lsigma_))
lsigma_ = 0.075;
if(!private_nh_.getParam("ogain", ogain_))
ogain_ = 3.0;
if(!private_nh_.getParam("lskip", lskip_))
lskip_ = 0;
if(!private_nh_.getParam("srr", srr_))
srr_ = 0.1;
if(!private_nh_.getParam("srt", srt_))
srt_ = 0.2;
if(!private_nh_.getParam("str", str_))
str_ = 0.1;
if(!private_nh_.getParam("stt", stt_))
stt_ = 0.2;
if(!private_nh_.getParam("linearUpdate", linearUpdate_))
linearUpdate_ = 1.0;
if(!private_nh_.getParam("angularUpdate", angularUpdate_))
angularUpdate_ = 0.5;
if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
temporalUpdate_ = -1.0;
if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
resampleThreshold_ = 0.5;
if(!private_nh_.getParam("particles", particles_))
particles_ = 30;
if(!private_nh_.getParam("xmin", xmin_))
xmin_ = -100.0;
if(!private_nh_.getParam("ymin", ymin_))
ymin_ = -100.0;
if(!private_nh_.getParam("xmax", xmax_))
xmax_ = 100.0;
if(!private_nh_.getParam("ymax", ymax_))
ymax_ = 100.0;
if(!private_nh_.getParam("delta", delta_))
delta_ = 0.05;
if(!private_nh_.getParam("occ_thresh", occ_thresh_))
occ_thresh_ = 0.25;
if(!private_nh_.getParam("llsamplerange", llsamplerange_))
llsamplerange_ = 0.01;
if(!private_nh_.getParam("llsamplestep", llsamplestep_))
llsamplestep_ = 0.01;
if(!private_nh_.getParam("lasamplerange", lasamplerange_))
lasamplerange_ = 0.005;
if(!private_nh_.getParam("lasamplestep", lasamplestep_))
lasamplestep_ = 0.005;
if(!private_nh_.getParam("tf_delay", tf_delay_))
tf_delay_ = transform_publish_period;
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period));
}
HectorMappingRos::HectorMappingRos()
: debugInfoProvider(0)
, hectorDrawings(0)
, lastGetMapUpdateIndex(-100)
, tfB_(0)
, map__publish_thread_(0)
, initial_pose_set_(false)
{
ros::NodeHandle private_nh_("~");
std::string mapTopic_ = "map";
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);
if (p_pub_drawings)
{
ROS_INFO("HectorSM publishing debug drawings");
hectorDrawings = new HectorDrawings();
}
if(p_pub_debug_output_)
{
ROS_INFO("HectorSM publishing debug info");
debugInfoProvider = new HectorDebugInfoProvider();
}
if(p_pub_odometry_)
{
odometryPublisher_ = node_.advertise<nav_msgs::Odometry>("scanmatch_odom", 50);
}
slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider);
slamProcessor->setUpdateFactorFree(p_update_factor_free_);
slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);
slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_);
slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);
int mapLevels = slamProcessor->getMapLevels();
mapLevels = 1;
for (int i = 0; i < mapLevels; ++i)
{
mapPubContainer.push_back(MapPublisherContainer());
slamProcessor->addMapMutex(i, new HectorMapMutex());
std::string mapTopicStr(mapTopic_);
if (i != 0)
{
mapTopicStr.append("_" + boost::lexical_cast<std::string>(i));
}
std::string mapMetaTopicStr(mapTopicStr);
mapMetaTopicStr.append("_metadata");
MapPublisherContainer& tmp = mapPubContainer[i];
tmp.mapPublisher_ = node_.advertise<nav_msgs::OccupancyGrid>(mapTopicStr, 1, true);
tmp.mapMetadataPublisher_ = node_.advertise<nav_msgs::MapMetaData>(mapMetaTopicStr, 1, true);
if ( (i == 0) && p_advertise_map_service_)
{
tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this);
}
setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i));
if ( i== 0){
mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
}
}
ROS_INFO("HectorSM p_base_frame_: %s", p_base_frame_.c_str());
ROS_INFO("HectorSM p_map_frame_: %s", p_map_frame_.c_str());
ROS_INFO("HectorSM p_odom_frame_: %s", p_odom_frame_.c_str());
ROS_INFO("HectorSM p_scan_topic_: %s", p_scan_topic_.c_str());
ROS_INFO("HectorSM p_use_tf_scan_transformation_: %s", p_use_tf_scan_transformation_ ? ("true") : ("false"));
ROS_INFO("HectorSM p_pub_map_odom_transform_: %s", p_pub_map_odom_transform_ ? ("true") : ("false"));
ROS_INFO("HectorSM p_scan_subscriber_queue_size_: %d", p_scan_subscriber_queue_size_);
ROS_INFO("HectorSM p_map_pub_period_: %f", p_map_pub_period_);
ROS_INFO("HectorSM p_update_factor_free_: %f", p_update_factor_free_);
ROS_INFO("HectorSM p_update_factor_occupied_: %f", p_update_factor_occupied_);
ROS_INFO("HectorSM p_map_update_distance_threshold_: %f ", p_map_update_distance_threshold_);
ROS_INFO("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_);
ROS_INFO("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_);
ROS_INFO("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_);
scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);
sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);
poseUpdatePublisher_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>(p_pose_update_topic_, 1, false);
posePublisher_ = node_.advertise<geometry_msgs::PoseStamped>("slam_out_pose", 1, false);
scan_point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud>("slam_cloud",1,false);
tfB_ = new tf::TransformBroadcaster();
ROS_ASSERT(tfB_);
/*
bool p_use_static_map_ = false;
if (p_use_static_map_){
mapSubscriber_ = node_.subscribe(mapTopic_, 1, &HectorMappingRos::staticMapCallback, this);
}
*/
initial_pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(node_, "initialpose", 2);
initial_pose_filter_ = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*initial_pose_sub_, tf_, p_map_frame_, 2);
initial_pose_filter_->registerCallback(boost::bind(&HectorMappingRos::initialPoseCallback, this, _1));
map__publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this, p_map_pub_period_));
map_to_odom_.setIdentity();
lastMapPublishTime = ros::Time(0,0);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "astar_navi");
ros::NodeHandle n;
ros::NodeHandle private_nh_("~");
double waypoint_velocity_kmph;
std::string map_topic;
private_nh_.param<double>("waypoint_velocity_kmph", waypoint_velocity_kmph, 5.0);
private_nh_.param<std::string>("map_topic", map_topic, "ring_ogm");
AstarSearch astar;
SearchInfo search_info;
// ROS subscribers
ros::Subscriber map_sub = n.subscribe(map_topic, 1, &SearchInfo::mapCallback, &search_info);
ros::Subscriber start_sub = n.subscribe("/current_pose", 1, &SearchInfo::currentPoseCallback, &search_info);
ros::Subscriber goal_sub = n.subscribe("/move_base_simple/goal", 1, &SearchInfo::goalCallback, &search_info);
// ROS publishers
ros::Publisher path_pub = n.advertise<nav_msgs::Path>("astar_path", 1, true);
ros::Publisher waypoints_pub = n.advertise<waypoint_follower::LaneArray>("lane_waypoints_array", 1, true);
ros::Publisher debug_pose_pub = n.advertise<geometry_msgs::PoseArray>("debug_pose_array", 1, true);
ros::Rate loop_rate(10);
while (ros::ok()) {
ros::spinOnce();
if (!search_info.getMapSet() || !search_info.getStartSet() || !search_info.getGoalSet()) {
loop_rate.sleep();
continue;
}
// Reset flag
search_info.reset();
auto start = std::chrono::system_clock::now();
// Execute astar search
bool result = astar.makePlan(search_info.getStartPose().pose, search_info.getGoalPose().pose, search_info.getMap());
auto end = std::chrono::system_clock::now();
auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
//std::cout << "astar msec: " << usec / 1000.0 << std::endl;
ROS_INFO("astar msec: %lf", usec / 1000.0);
if(result) {
ROS_INFO("Found GOAL!");
publishPathAsWaypoints(waypoints_pub, astar.getPath(), waypoint_velocity_kmph);
#if DEBUG
astar.publishPoseArray(debug_pose_pub, "/map");
path_pub.publish(astar.getPath());
astar.broadcastPathTF();
#endif
} else {
ROS_INFO("can't find goal...");
#if DEBUG
astar.publishPoseArray(debug_pose_pub, "/map"); // debug
path_pub.publish(astar.getPath());
#endif
}
astar.reset();
loop_rate.sleep();
}
return 0;
}
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = scan.header.frame_id;
ident.stamp_ = scan.header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
#ifdef LOGTOFILE
writeLaserPoseStamped(laser_pose);
#endif
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
double yaw = tf::getYaw(laser_pose.getRotation());
GMapping::OrientedPoint gmap_pose(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
ROS_DEBUG("laser's pose wrt base: %.3f %.3f %.3f",
laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, scan.angle_min);
tf::Stamped<tf::Quaternion> min_q(q, scan.header.stamp,
scan.header.frame_id);
q.setRPY(0.0, 0.0, scan.angle_max);
tf::Stamped<tf::Quaternion> max_q(q, scan.header.stamp,
scan.header.frame_id);
try
{
tf_.transformQuaternion(base_frame_, min_q, min_q);
tf_.transformQuaternion(base_frame_, max_q, max_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return false;
}
#ifdef LOGTOFILE
writeMinMax(min_q);
writeMinMax(max_q);
#endif
gsp_laser_beam_count_ = scan.ranges.size();
double angle_min = tf::getYaw(min_q);
double angle_max = tf::getYaw(max_q);
gsp_laser_angle_increment_ = scan.angle_increment;
ROS_DEBUG("Laser angles in base frame: min: %.3f max: %.3f inc: %.3f", angle_min, angle_max, gsp_laser_angle_increment_);
// setting maxRange and maxUrange here so we can set a reasonable default
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(gsp_laser_angle_increment_),
gmap_pose,
0.0,
maxRange_);
ROS_ASSERT(gsp_laser_);
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);
/// @todo Expose setting an initial pose
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,time(NULL));
ROS_INFO("Initialization complete");
return true;
}
// Constructor
objectDetect::objectDetect() :
cpu_detector_(NULL),
#if defined(HAS_GPU)
gpu_detector_(NULL),
#endif
object_class("car")
{
ros::NodeHandle private_nh_("~");
private_nh_.param("overlap_threshold", overlap_threshold_, 0.5);
private_nh_.param("num_threads", num_threads_, 8);
private_nh_.param("lambda", lambda_, 10);
private_nh_.param("num_cells", num_cells_, NUM_CELLS);
private_nh_.param("num_bins", num_bins_, NUM_BINS);
private_nh_.param("val_of_tuncate", val_of_truncate_, 0.2);
private_nh_.param<std::string>("image_raw_topic", image_topic_name, "/image_raw");
if (!private_nh_.getParam("detection_class_name", object_class)) {
object_class = "car";
}
#if defined(HAS_GPU)
if (!private_nh_.getParam("use_gpu", use_gpu)) {
use_gpu = false;
}
#endif
std::string default_model;
// switch (type) {
// case DetectType::CAR:
// default_model = std::string(STR(MODEL_DIR) "car_2008.xml");
// break;
// case DetectType::PEDESTRIAN:
// default_model = std::string(STR(MODEL_DIR) "person.xml");
// break;
// default:
// break;
// }
if (object_class == "car") {
default_model = std::string(STR(MODEL_DIR) "car_2008.xml");
}
else if (object_class == "person") {
default_model = std::string(STR(MODEL_DIR) "person.xml");
}
private_nh_.param("model_file", model_file_, default_model);
std::vector<std::string> model_filenames;
model_filenames.clear();
model_filenames.push_back(model_file_);
cpu_detector_ = new DPMOCVCPULatentSvmDetector(model_filenames);
if (!private_nh_.getParam("score_threshold", score_threshold_))
{
score_threshold_ = SCORE_THRESHOLD;
}
#if defined(HAS_GPU)
gpu_detector_ = new DPMOCVGPULatentSvmDetector(model_filenames, (float)score_threshold_);
#endif
}
SlamKarto::SlamKarto() :
got_map_(false),
laser_count_(0),
transform_thread_(NULL),
marker_count_(0)
{
map_to_odom_.setIdentity();
// Retrieve parameters
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("use_robust_kernel", use_robust_kernel_))
{
use_robust_kernel_ = false;
}
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))
tmp = 5.0;
map_update_interval_.fromSec(tmp);
if(!private_nh_.getParam("resolution", resolution_))
{
// Compatibility with slam_gmapping, which uses "delta" to mean
// resolution
if(!private_nh_.getParam("delta", resolution_))
resolution_ = 0.05;
}
double transform_publish_period;
private_nh_.param("transform_publish_period", transform_publish_period, 0.05);
// Set up advertisements and subscriptions
tfB_ = new tf::TransformBroadcaster();
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamKarto::laserCallback, this, _1));
marker_publisher_ = node_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",1);
// Create a thread to periodically publish the latest map->odom
// transform; it needs to go out regularly, uninterrupted by potentially
// long periods of computation in our main loop.
transform_thread_ = new boost::thread(boost::bind(&SlamKarto::publishLoop, this, transform_publish_period));
// Initialize Karto structures
mapper_ = new karto::Mapper();
dataset_ = new karto::Dataset();
// Setting General Parameters from the Parameter Server
bool use_scan_matching;
if(private_nh_.getParam("use_scan_matching", use_scan_matching))
mapper_->setParamUseScanMatching(use_scan_matching);
bool use_scan_barycenter;
if(private_nh_.getParam("use_scan_barycenter", use_scan_barycenter))
mapper_->setParamUseScanBarycenter(use_scan_barycenter);
double minimum_travel_distance;
if(private_nh_.getParam("minimum_travel_distance", minimum_travel_distance))
mapper_->setParamMinimumTravelDistance(minimum_travel_distance);
double minimum_travel_heading;
if(private_nh_.getParam("minimum_travel_heading", minimum_travel_heading))
mapper_->setParamMinimumTravelHeading(minimum_travel_heading);
int scan_buffer_size;
if(private_nh_.getParam("scan_buffer_size", scan_buffer_size))
mapper_->setParamScanBufferSize(scan_buffer_size);
double scan_buffer_maximum_scan_distance;
if(private_nh_.getParam("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance))
mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance);
double link_match_minimum_response_fine;
if(private_nh_.getParam("link_match_minimum_response_fine", link_match_minimum_response_fine))
mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine);
double link_scan_maximum_distance;
if(private_nh_.getParam("link_scan_maximum_distance", link_scan_maximum_distance))
mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance);
double loop_search_maximum_distance;
if(private_nh_.getParam("loop_search_maximum_distance", loop_search_maximum_distance))
mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance);
bool do_loop_closing;
if(private_nh_.getParam("do_loop_closing", do_loop_closing))
mapper_->setParamDoLoopClosing(do_loop_closing);
int loop_match_minimum_chain_size;
if(private_nh_.getParam("loop_match_minimum_chain_size", loop_match_minimum_chain_size))
mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size);
double loop_match_maximum_variance_coarse;
if(private_nh_.getParam("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse))
mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse);
double loop_match_minimum_response_coarse;
if(private_nh_.getParam("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse))
mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse);
double loop_match_minimum_response_fine;
if(private_nh_.getParam("loop_match_minimum_response_fine", loop_match_minimum_response_fine))
mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine);
// Setting Correlation Parameters from the Parameter Server
double correlation_search_space_dimension;
if(private_nh_.getParam("correlation_search_space_dimension", correlation_search_space_dimension))
mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension);
double correlation_search_space_resolution;
if(private_nh_.getParam("correlation_search_space_resolution", correlation_search_space_resolution))
mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution);
double correlation_search_space_smear_deviation;
if(private_nh_.getParam("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation))
mapper_->setParamCorrelationSearchSpaceSmearDeviation(correlation_search_space_smear_deviation);
// Setting Correlation Parameters, Loop Closure Parameters from the Parameter Server
double loop_search_space_dimension;
if(private_nh_.getParam("loop_search_space_dimension", loop_search_space_dimension))
mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension);
double loop_search_space_resolution;
if(private_nh_.getParam("loop_search_space_resolution", loop_search_space_resolution))
mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution);
double loop_search_space_smear_deviation;
if(private_nh_.getParam("loop_search_space_smear_deviation", loop_search_space_smear_deviation))
mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation);
// Setting Scan Matcher Parameters from the Parameter Server
double distance_variance_penalty;
if(private_nh_.getParam("distance_variance_penalty", distance_variance_penalty))
mapper_->setParamDistanceVariancePenalty(distance_variance_penalty);
double angle_variance_penalty;
if(private_nh_.getParam("angle_variance_penalty", angle_variance_penalty))
mapper_->setParamAngleVariancePenalty(angle_variance_penalty);
double fine_search_angle_offset;
if(private_nh_.getParam("fine_search_angle_offset", fine_search_angle_offset))
mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);
double coarse_search_angle_offset;
if(private_nh_.getParam("coarse_search_angle_offset", coarse_search_angle_offset))
mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset);
double coarse_angle_resolution;
if(private_nh_.getParam("coarse_angle_resolution", coarse_angle_resolution))
mapper_->setParamCoarseAngleResolution(coarse_angle_resolution);
double minimum_angle_penalty;
if(private_nh_.getParam("minimum_angle_penalty", minimum_angle_penalty))
mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty);
double minimum_distance_penalty;
if(private_nh_.getParam("minimum_distance_penalty", minimum_distance_penalty))
mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty);
bool use_response_expansion;
if(private_nh_.getParam("use_response_expansion", use_response_expansion))
mapper_->setParamUseResponseExpansion(use_response_expansion);
// Set solver to be used in loop closure
solver_ = new G2OSolver();
solver_->useRobustKernel(use_robust_kernel_);
mapper_->SetScanSolver(solver_);
}